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 __init__(self):
self.PORT_NUMBER = 5004
self.DATA_SIZE = 1024
self.DEFAULT_PULSES_PER_SECOND = 0
self.DEFAULT_HOLD_MODE = "on"
self.DEFAULT_STOP_MODE = "brake"
self.DEFAULT_REGULATION_MODE = True
self.MOTOR_STARTING_POSITION = 0
self.system_on = False
self.YAW_LEFT = "yaw_left "
self.YAW_RIGHT = "yaw_right"
self.socket = None
self.motor_lock = threading.Lock()
self.unpacker = struct.Struct('f f')
self.right_track_motor=Motor(port=Motor.PORT.A)
self.left_track_motor=Motor(port=Motor.PORT.B)
#self.servo_arm_pitch_motor=Motor(port=Motor.PORT.C)
#self.servo_arm_roll_motor=Motor(port=Motor.PORT.D)
self.camera_fov = 640
self.camera_av = self.camera_fov/2
self.face_x = 0
self.face_y = 0
self.kp = 0.15625 # Constant for the proportional controller
self.ki = 0 # Constant for the integral controller
self.kd = 0 # Constant for the differential controller
self.offset = self.camera_av # Offtset used to calculate error
self.tp = 0 # Turning power of motors when there is no error
self.previous_dt = 0
self.integral = 0
self.previous_err = 0
self.derivative = 0
self.coordinates_time_recv = 0
self.timeout = 0.2 #seconds
self.face_coordinates_lock = threading.Lock()
self.system_on_lock = threading.Lock()
self.update_coordinates_thread = threading.Thread(target = self.update_face_coordinates)
self.update_coordinates_thread.start()
print "Setting up connection.\n"
self.initialise_connection()
print "Initialising Motors..\n"
self.initialise_motors()
print "Awaiting new connection...\n"
self.update_face_coordinates()
def __init__(self):
self.PORT_NUMBER = 5004
self.DATA_SIZE = 1024
self.DEFAULT_PULSES_PER_SECOND = 0
self.DEFAULT_HOLD_MODE = "on"
self.DEFAULT_STOP_MODE = "brake"
self.DEFAULT_REGULATION_MODE = True
self.MOTOR_STARTING_POSITION = 0
self.system_on = False
self.YAW_LEFT = "yaw_left "
self.YAW_RIGHT = "yaw_right"
self.socket = None
self.motor_lock = threading.Lock()
self.unpacker = struct.Struct('f f')
self.right_track_motor = Motor(port=Motor.PORT.A)
self.left_track_motor = Motor(port=Motor.PORT.B)
#self.servo_arm_pitch_motor=Motor(port=Motor.PORT.C)
#self.servo_arm_roll_motor=Motor(port=Motor.PORT.D)
self.camera_fov = 640
self.camera_av = self.camera_fov / 2
self.face_x = 0
self.face_y = 0
self.kp = 0.15625 # Constant for the proportional controller
self.ki = 0 # Constant for the integral controller
self.kd = 0 # Constant for the differential controller
self.offset = self.camera_av # Offtset used to calculate error
self.tp = 0 # Turning power of motors when there is no error
self.previous_dt = 0
self.integral = 0
self.previous_err = 0
self.derivative = 0
self.coordinates_time_recv = 0
self.timeout = 0.2 #seconds
self.face_coordinates_lock = threading.Lock()
self.system_on_lock = threading.Lock()
self.update_coordinates_thread = threading.Thread(
target=self.update_face_coordinates)
self.update_coordinates_thread.start()
print "Setting up connection.\n"
self.initialise_connection()
print "Initialising Motors..\n"
self.initialise_motors()
print "Awaiting new connection...\n"
self.update_face_coordinates()
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
def __init__(self):
self.left=Motor(port=Motor.PORT.B)
self.right=Motor(port=Motor.PORT.C)
self.reset()
self.gyro=Gyro()
class Mover():
# Moving motors
left=None;
right=None;
# Sensors
gyro=None;
# Paras
Rrio=2.15; # 60/28=2.14285
# Configs
angle=240;
rmp_sp=200;
run_sp=500; # running speed
t_sp=400; # turning speed
def __init__(self):
self.left=Motor(port=Motor.PORT.B);
self.right=Motor(port=Motor.PORT.C);
self.left.reset();
self.right.reset();
self.gyro=GyroSensor();
def readAngle(self,RESET=0):
if(RESET==1):
self.gyro.rate;
return self.gyro.ang;
def reset(self):
self.left.reset();
self.right.reset();
def runForward(self):
self.reset();
self.left.setup_forever(self.run_sp,speed_regulation = True);
self.right.setup_forever(self.run_sp,speed_regulation = True);
self.left.start();
self.right.start();
def runBackward(self):
self.reset();
self.left.setup_forever(-self.run_sp,speed_regulation = True);
self.right.setup_forever(-self.run_sp,speed_regulation = True);
self.left.start();
self.right.start();
def stop(self):
self.left.stop();
self.right.stop();
def turnRightbyAngle(self, ang):
angtruth = 0;
angtmp = ang;
# adjust loop
while (abs(angtmp)>3):
angtruth=self.oneAngleTurnRight(angtmp);
angtmp=angtmp-angtruth;
#print "Diff: "+str(angtmp);
def oneAngleTurnRight(self, ang):
before=self.readAngle(RESET=1);
#print "Before right turn: "+str(before);
self.reset();
self.left.position=0;
self.left.setup_position_limited(position_sp=int(ang*self.Rrio), speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp);
self.right.position=0;
self.right.setup_position_limited(position_sp=-int(ang*self.Rrio), speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp);
self.left.start();
self.right.start();
time.sleep(2);
self.stop();
after=self.readAngle();
#print "After right turn: "+str(after);
return after - before;
def turnLeftbyAngle(self, ang):
angtruth = 0;
angtmp = ang;
# adjust loop
while (abs(angtmp)>3):
angtruth=self.oneAngleTurnLeft(angtmp);
angtmp=angtmp+angtruth;
print "Diff: "+str(angtmp);
def oneAngleTurnLeft(self, ang):
before=self.readAngle(RESET=1);
print "Before right turn: "+str(before);
self.reset();
self.right.position=0;
self.right.setup_position_limited(position_sp=int(ang*self.Rrio), speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp);
self.left.position=0;
self.left.setup_position_limited(position_sp=-int(ang*self.Rrio), speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp);
self.right.start();
self.left.start();
time.sleep(2);
self.stop();
after=self.readAngle();
print "After right turn: "+str(after);
return after - before;
class Ev3_Motor_Server:
def __init__(self):
self.PORT_NUMBER = 5004
self.DATA_SIZE = 1024
self.DEFAULT_PULSES_PER_SECOND = 0
self.DEFAULT_HOLD_MODE = "on"
self.DEFAULT_STOP_MODE = "brake"
self.DEFAULT_REGULATION_MODE = True
self.MOTOR_STARTING_POSITION = 0
self.system_on = False
self.YAW_LEFT = "yaw_left "
self.YAW_RIGHT = "yaw_right"
self.socket = None
self.motor_lock = threading.Lock()
self.unpacker = struct.Struct('f f')
self.right_track_motor = Motor(port=Motor.PORT.A)
self.left_track_motor = Motor(port=Motor.PORT.B)
#self.servo_arm_pitch_motor=Motor(port=Motor.PORT.C)
#self.servo_arm_roll_motor=Motor(port=Motor.PORT.D)
self.camera_fov = 640
self.camera_av = self.camera_fov / 2
self.face_x = 0
self.face_y = 0
self.kp = 0.15625 # Constant for the proportional controller
self.ki = 0 # Constant for the integral controller
self.kd = 0 # Constant for the differential controller
self.offset = self.camera_av # Offtset used to calculate error
self.tp = 0 # Turning power of motors when there is no error
self.previous_dt = 0
self.integral = 0
self.previous_err = 0
self.derivative = 0
self.coordinates_time_recv = 0
self.timeout = 0.2 #seconds
self.face_coordinates_lock = threading.Lock()
self.system_on_lock = threading.Lock()
self.update_coordinates_thread = threading.Thread(
target=self.update_face_coordinates)
self.update_coordinates_thread.start()
print "Setting up connection.\n"
self.initialise_connection()
print "Initialising Motors..\n"
self.initialise_motors()
print "Awaiting new connection...\n"
self.update_face_coordinates()
def initialise_connection(self):
hostname = socket.gethostbyname('0.0.0.0')
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket.bind((hostname, self.PORT_NUMBER))
self.socket.listen(1)
def initialise_motors(self):
self.set_up_motor(self.right_track_motor)
self.set_up_motor(self.left_track_motor)
#set_up_motor(self.servo_arm_pitch_motor)
#set_up_motor(self.servo_arm_roll_motor)
def set_up_motor(self, motor):
motor.reset()
motor.position = self.MOTOR_STARTING_POSITION
motor.regulation_mode = self.DEFAULT_REGULATION_MODE
motor.pulses_per_second_sp = self.DEFAULT_PULSES_PER_SECOND
motor.hold_mode = self.DEFAULT_HOLD_MODE
motor.stop_mode = self.DEFAULT_STOP_MODE
def current_time(self):
return time.time() / 1000 # In seconds
def set_face_coordinates(self, x, y):
with self.face_coordinates_lock:
self.x = x
self.y = y
def get_face_coordinates(self):
return self.x, self.y
def set_system_on(self, state):
with self.system_on_lock:
self.system_on = state
def get_system_on(self):
return self.system_on
def calculate_x_axis_pid(self):
print "time 1" + str(self.current_time())
face_x, face_y = self.get_face_coordinates()
dt = self.current_time() - self.previous_dt
error = face_x - self.offset
self.integral = self.integral + (error * dt)
self.derivative = (error - self.previous_err) / dt
turning_power = (self.kp * error) + (self.ki * self.integral) + (
self.kd * self.derivative)
left_motor_power = self.tp + turning_power # These depend on the inital
right_motor_power = self.tp - turning_power # orientation of the motors
print str(turning_power)
self.right_track_motor.pulses_per_second_sp = right_motor_power
self.left_track_motor.pulses_per_second_sp = left_motor_power
self.previous_dt = dt
self.previous_err = error
print "time 2" + str(self.current_time())
def stop_x_axis_motors(self):
self.right_track_motor.stop()
self.left_track_motor.stop()
def start_x_axis_motors(self):
self.right_track_motor.start()
self.left_track_motor.start()
def motor_control(self):
self.previous_dt = self.current_time()
try:
if self.coordinates_time_recv > 0:
self.calculate_x_axis_pid()
self.start_x_axis_motors()
else:
self.previous_dt = time.time()
self.stop_x_axis_motors()
except (KeyboardInterrupt, SystemExit):
#self.cleanup()
raise
def update_face_coordinates(self):
conn, addr = self.socket.accept()
print conn, addr
self.coordinates_time_recv = -1
print "Connection address: ", addr
self.set_system_on(True)
while self.get_system_on():
try:
data = conn.recv(self.unpacker.size) #data size
if not data:
if self.current_time() > (
self.coordinates_time_recv * self.timeout) and (
self.coordinates_time_recv > -1):
self.coordinates_time_recv = -1
raise CoordinatesTimeOut()
else:
face_x, face_y = self.unpacker.unpack(data)
print face_x, face_y
self.coordinates_time_recv = self.current_time()
self.set_face_coordinates(face_x, face_y)
print "Face coordinates recieved at: " + str(
self.coordinates_time_recv) + "s, Position: (" + str(
face_x) + "," + str(face_y) + ")\n"
self.motor_control()
except CoordinatesTimeOut, e:
self.coordinates_time_recv = -1
self.stop_x_axis_motors()
print "ERROR: No new coordinates recieved!\nThe current coordinates are no longer valid, stopping motors!\n" + e
except KeyboardInterrupt, SystemExit:
self.cleanup()
conn.close()
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)
import socket
import ev3.ev3dev
import time
from ev3.ev3dev import Motor
rightTrackMotor=Motor(port=Motor.PORT.A)
leftTrackMotor=Motor(port=Motor.PORT.B)
servoArmPitchMotor=Motor(port=Motor.PORT.C)
servoArmRollMotor=Motor(port=Motor.PORT.D)
print "Motors initialised."
def setUp(motor):
motor.reset()
motor.regulation_mode = True
motor.pulses_per_second_sp = 30
setUp(rightTrackMotor)
setUp(leftTrackMotor)
setUp(servoArmPitchMotor)
setUp(servoArmRollMotor)
print "Motors set up."
#def yaw(direction):
#left/right
#def roll(direction):
#left/right
#def pitch(direction):
The program is stopped by pressing the baecon button.
"""
from ev3.ev3dev import Motor, NoSuchMotorError
from ev3.lego import InfraredSensor
from ev3.event_loop import EventLoop
ir = InfraredSensor()
motors = []
for port in "ABCD":
try:
motor = Motor(port=port)
motor.regulation_mode = False
except NoSuchMotorError:
motor = None
motors.append(motor)
buttons = [(ir.REMOTE.RED_UP, ir.REMOTE.RED_DOWN), (ir.REMOTE.BLUE_UP, ir.REMOTE.BLUE_DOWN)]
def ir_changed(event):
for channel in range(2):
state = event.evaluation[channel]
if state == ir.REMOTE.BAECON_MODE_ON:
for m in motors:
if m is not None:
m.stop()
def __init__(self,port):
self.motor=Motor(port=port)
self.motor.reset()
__author__ = 'stas Zytkiewicz [email protected]' from ev3.ev3dev import Motor for p in (Motor.PORT.A, Motor.PORT.B, Motor.PORT.C, Motor.PORT.D): try: m = Motor(port=p) m.stop() m.reset() except Exception, info: print info else: print "Motor %s stopped" % p
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.setup_forever(200,speed_regulation = True)
self.d.start()
time.sleep(5)
self.d.stop()
def test_run_forever(self):
self.d.run_forever(50, speed_regulation=False)
time.sleep(5)
self.d.stop()
self.d.run_forever(200, speed_regulation=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 __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)
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
app.debug = True
# Create a random secrey key so we can use sessions
app.config['SECRET_KEY'] = os.urandom(12)
from ev3.ev3dev import Tone
alarm = Tone()
#head = None#Motor(port=Motor.PORT.A)
right_wheel = None
left_wheel = None
button = None
ir_sensor = None
color_sensor = None
try:
right_wheel = Motor(port=Motor.PORT.B)
left_wheel = Motor(port=Motor.PORT.C)
button = TouchSensor()
#ir_sensor = InfraredSensor()
color_sensor = ColorSensor()
alarm.play(200)
except Exception as e:
alarm.play(200)
alarm.play(200)
raise e
right_wheel.position = 0
left_wheel.position = 0
right_wheel.reset()
left_wheel.reset()
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')
C 2 red
D 2 blue
The program is stopped by pressing the baecon button.
"""
from ev3.ev3dev import Motor, NoSuchMotorError
from ev3.lego import InfraredSensor
from ev3.event_loop import EventLoop
ir = InfraredSensor()
motors = []
for port in 'ABCD':
try:
motor = Motor(port=port)
motor.regulation_mode = False
except NoSuchMotorError:
motor = None
motors.append(motor)
buttons = [
(ir.REMOTE.RED_UP, ir.REMOTE.RED_DOWN),
(ir.REMOTE.BLUE_UP, ir.REMOTE.BLUE_DOWN),
]
def ir_changed(event):
for channel in range(2):
state = event.evaluation[channel]
if state == ir.REMOTE.BAECON_MODE_ON:
from ev3.ev3dev import Motor
import time
def init_motor(motor):
print ("Initializing motor")
motor.reset()
motor.run_mode = 'forever'
motor.stop_mode = Motor.STOP_MODE.BRAKE
motor.regulation_mode = True
motor.pulses_per_second_sp = 0
motor.start()
print ("Motor Initialized")
return;
a = Motor(port=Motor.PORT.A)
b = Motor(port=Motor.PORT.B)
init_motor(a)
init_motor(b)
a.pulses_per_second_sp = 2000
b.pulses_per_second_sp = 2000
time.sleep(5)
a.pulses_per_second_sp = 0
b.pulses_per_second_sp = 0;
class Driver():
# Wheel motors
left=None
right=None
# Sensors
gyro=None
# Paras
Rrio=2.15 # R_robot/R_wheel: 60/28=2.14285
Wper=175.93 # perimeter of the powering wheels: 175.92919 mm
# Configs
angle=240
rmp_sp=200
run_sp=500 # running speed
t_sp=400 # turning speed
t_accuracy=2 # turning accuracy for Gyro, in degree
r_accuracy=5 # running accuracy for Motor position, in degree
adjust_sp = [100, 50]
def __init__(self):
self.left=Motor(port=Motor.PORT.B)
self.right=Motor(port=Motor.PORT.C)
self.reset()
self.gyro=Gyro()
def reset(self):
self.left.reset()
self.right.reset()
def runForward(self):
self.reset()
self.left.setup_forever(self.run_sp,speed_regulation = True)
self.right.setup_forever(self.run_sp,speed_regulation = True)
self.left.start()
self.right.start()
def forwardbyDistance(self, dist): # distance must be in cm, and positive
if (dist == 0):
return
angle = int(abs(dist*10)/self.Wper*360)
self.forwardbyAngle(angle)
#self.forwardbySecond()
def forwardbyAngle(self, angle):
angtruth = [0, 0]
angtmp = [angle, angle]
# moving and adjusting loop
loop_counter = 0;
while(abs(angtmp[0])>=self.r_accuracy and abs(angtmp[1]) >=self.r_accuracy):
angtruth = self.oneAngleForward(angtmp[0], angtmp[1])
angtmp[0]=angtmp[0]-angtruth[0]
angtmp[1]=angtmp[1]-angtruth[1]
print "fore New angtmp: "+ str(angtmp)
def oneAngleForward(self, left_ang, right_ang):
# run the main movement
self.reset()
self.left.setup_position_limited(position_sp=left_ang, speed_sp=self.run_sp,
stop_mode=Motor.STOP_MODE.HOLD, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.right.setup_position_limited(position_sp=right_ang, speed_sp=self.run_sp,
stop_mode=Motor.STOP_MODE.HOLD, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.left.start()
self.right.start()
time.sleep(10) # need to figure out how much needed
self.stop()
# adjust using low speed mode
for v_adj in self.adjust_sp:
self.left.setup_position_limited(position_sp=left_ang, speed_sp=v_adj,
stop_mode=Motor.STOP_MODE.HOLD, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.right.setup_position_limited(position_sp=right_ang, speed_sp=v_adj,
stop_mode=Motor.STOP_MODE.HOLD, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.left.start()
self.right.start()
time.sleep(1)
return [self.left.position, self.right.position]
def forwardbySecond(self, sec):
self.reset();
self.left.setup_time_limited(time_sp=sec, speed_sp=self.run_sp, speed_regulation=True)
self.right.setup_time_limited(time_sp=sec, speed_sp=self.run_sp, speed_regulation=True)
self.left.start()
self.right.start()
time.sleep(sec)
self.stop()
def runBackward(self):
self.reset()
self.left.setup_forever(-self.run_sp,speed_regulation=True)
self.right.setup_forever(-self.run_sp,speed_regulation=True)
self.left.start()
self.right.start()
def backwardbyDistance(self, dist): # distance must be in cm, and positive
if (dist == 0):
return
angle = -int(abs(dist*10)/self.Wper*360)
self.backwardbyAngle(angle)
def backwardbyAngle(self, angle):
angtruth = [0, 0]
angtmp = [angle, angle]
# moving and adjusting loop
while(abs(angtmp[0])>=self.r_accuracy and abs(angtmp[1]) >=self.r_accuracy):
angtruth = self.oneAngleForward(angtmp[0], angtmp[1])
angtmp[0] = angtmp[0]+angtruth[0]
angtmp[1] = angtmp[1]+angtruth[1]
print "back New angtmp: "+ str(angtmp)
def oneAngleBackward(self, left_ang, right_ang):
self.reset()
self.left.setup_position_limited(position_sp=-left_ang, speed_sp=self.run_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.right.setup_position_limited(position_sp=-right_ang, speed_sp=self.run_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.left.start()
self.right.start()
time.sleep(10)
self.stop()
# adjust using low speed mode
for v_adj in self.adjust_sp:
self.left.setup_position_limited(position_sp=-left_ang, speed_sp=v_adj,
stop_mode=Motor.STOP_MODE.HOLD, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.right.setup_position_limited(position_sp=-right_ang, speed_sp=v_adj,
stop_mode=Motor.STOP_MODE.HOLD, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.left.start()
self.right.start()
time.sleep(1)
return [self.left.position, self.right.position]
def turnRightbyAngle(self, ang):
angtruth = 0
angtmp = ang
# moving and adjusting loop
while (abs(angtmp)>self.t_accuracy):
angtruth=self.oneAngleTurnRight(angtmp)
angtmp=angtmp-angtruth
#print "Diff: "+str(angtmp)
def oneAngleTurnRight(self, ang):
before=self.gyro.readAngle()
#print "Before right turn: "+str(before)
self.reset()
self.left.setup_position_limited(position_sp=int(ang*self.Rrio), speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.right.setup_position_limited(position_sp=-int(ang*self.Rrio), speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.left.start()
self.right.start()
time.sleep(2)
self.stop()
after=self.gyro.readAngle()
#print "After right turn: "+str(after)
return after - before
def turnLeftbyAngle(self, ang):
angtruth = 0
angtmp = ang
# moving and adjusting loop
while (abs(angtmp)>self.t_accuracy):
angtruth=self.oneAngleTurnLeft(angtmp)
angtmp=angtmp+angtruth
print "Diff: "+str(angtmp)
def oneAngleTurnLeft(self, ang):
before=self.gyro.readAngle()
#print "Before right turn: "+str(before)
self.reset()
self.right.setup_position_limited(position_sp=int(ang*self.Rrio), speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.left.setup_position_limited(position_sp=-int(ang*self.Rrio), speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.right.start()
self.left.start()
time.sleep(2)
self.stop()
after=self.gyro.readAngle()
#print "After right turn: "+str(after)
return after - before
def stop(self):
self.left.stop()
self.right.stop()
def __init__(self,*args, **kwargs):
super(TestMotor, self).__init__(*args, **kwargs)
self.d=Motor(port=Motor.PORT.A)
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"
def __init__(self):
self.left = Motor(port=Motor.PORT.B)
self.right = Motor(port=Motor.PORT.C)
self.left.reset()
self.right.reset()
self.gyro = GyroSensor()
class Mover():
# Moving motors
left = None
right = None
# Sensors
gyro = None
# Paras
Rrio = 2.15
# 60/28=2.14285
# Configs
angle = 240
rmp_sp = 200
run_sp = 500
# running speed
t_sp = 400
# turning speed
def __init__(self):
self.left = Motor(port=Motor.PORT.B)
self.right = Motor(port=Motor.PORT.C)
self.left.reset()
self.right.reset()
self.gyro = GyroSensor()
def readAngle(self, RESET=0):
if (RESET == 1):
self.gyro.rate
return self.gyro.ang
def reset(self):
self.left.reset()
self.right.reset()
def runForward(self):
self.reset()
self.left.setup_forever(self.run_sp, speed_regulation=True)
self.right.setup_forever(self.run_sp, speed_regulation=True)
self.left.start()
self.right.start()
def runBackward(self):
self.reset()
self.left.setup_forever(-self.run_sp, speed_regulation=True)
self.right.setup_forever(-self.run_sp, speed_regulation=True)
self.left.start()
self.right.start()
def stop(self):
self.left.stop()
self.right.stop()
def turnRightbyAngle(self, ang):
angtruth = 0
angtmp = ang
# adjust loop
while (abs(angtmp) > 3):
angtruth = self.oneAngleTurnRight(angtmp)
angtmp = angtmp - angtruth
#print "Diff: "+str(angtmp);
def oneAngleTurnRight(self, ang):
before = self.readAngle(RESET=1)
#print "Before right turn: "+str(before);
self.reset()
self.left.position = 0
self.left.setup_position_limited(position_sp=int(ang * self.Rrio),
speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE,
ramp_up_sp=self.rmp_sp,
ramp_down_sp=self.rmp_sp)
self.right.position = 0
self.right.setup_position_limited(position_sp=-int(ang * self.Rrio),
speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE,
ramp_up_sp=self.rmp_sp,
ramp_down_sp=self.rmp_sp)
self.left.start()
self.right.start()
time.sleep(2)
self.stop()
after = self.readAngle()
#print "After right turn: "+str(after);
return after - before
def turnLeftbyAngle(self, ang):
angtruth = 0
angtmp = ang
# adjust loop
while (abs(angtmp) > 3):
angtruth = self.oneAngleTurnLeft(angtmp)
angtmp = angtmp + angtruth
print "Diff: " + str(angtmp)
def oneAngleTurnLeft(self, ang):
before = self.readAngle(RESET=1)
print "Before right turn: " + str(before)
self.reset()
self.right.position = 0
self.right.setup_position_limited(position_sp=int(ang * self.Rrio),
speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE,
ramp_up_sp=self.rmp_sp,
ramp_down_sp=self.rmp_sp)
self.left.position = 0
self.left.setup_position_limited(position_sp=-int(ang * self.Rrio),
speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE,
ramp_up_sp=self.rmp_sp,
ramp_down_sp=self.rmp_sp)
self.right.start()
self.left.start()
time.sleep(2)
self.stop()
after = self.readAngle()
print "After right turn: " + str(after)
return after - before
app.debug = True
# Create a random secrey key so we can use sessions
app.config['SECRET_KEY'] = os.urandom(12)
from ev3.ev3dev import Tone
alarm = Tone()
#head = None#Motor(port=Motor.PORT.A)
right_wheel = None
left_wheel = None
button = None
ir_sensor = None
color_sensor = None
try:
right_wheel = Motor(port=Motor.PORT.B)
left_wheel = Motor(port=Motor.PORT.C)
button = TouchSensor()
#ir_sensor = InfraredSensor()
color_sensor = ColorSensor()
alarm.play(200)
except Exception as e:
alarm.play(200)
alarm.play(200)
raise e
right_wheel.position = 0
left_wheel.position = 0
right_wheel.reset()
left_wheel.reset()
def __init__(self):
self.left=Motor(port=Motor.PORT.B);
self.right=Motor(port=Motor.PORT.C);
self.left.reset();
self.right.reset();
self.gyro=GyroSensor();
def __init__(self, *args, **kwargs):
super(TestMotor, self).__init__(*args, **kwargs)
self.d = Motor(port=Motor.PORT.A)
class Ev3_Motor_Server:
def __init__(self):
self.PORT_NUMBER = 5004
self.DATA_SIZE = 1024
self.DEFAULT_PULSES_PER_SECOND = 0
self.DEFAULT_HOLD_MODE = "on"
self.DEFAULT_STOP_MODE = "brake"
self.DEFAULT_REGULATION_MODE = True
self.MOTOR_STARTING_POSITION = 0
self.system_on = False
self.YAW_LEFT = "yaw_left "
self.YAW_RIGHT = "yaw_right"
self.socket = None
self.motor_lock = threading.Lock()
self.unpacker = struct.Struct('f f')
self.right_track_motor=Motor(port=Motor.PORT.A)
self.left_track_motor=Motor(port=Motor.PORT.B)
#self.servo_arm_pitch_motor=Motor(port=Motor.PORT.C)
#self.servo_arm_roll_motor=Motor(port=Motor.PORT.D)
self.camera_fov = 640
self.camera_av = self.camera_fov/2
self.face_x = 0
self.face_y = 0
self.kp = 0.15625 # Constant for the proportional controller
self.ki = 0 # Constant for the integral controller
self.kd = 0 # Constant for the differential controller
self.offset = self.camera_av # Offtset used to calculate error
self.tp = 0 # Turning power of motors when there is no error
self.previous_dt = 0
self.integral = 0
self.previous_err = 0
self.derivative = 0
self.coordinates_time_recv = 0
self.timeout = 0.2 #seconds
self.face_coordinates_lock = threading.Lock()
self.system_on_lock = threading.Lock()
self.update_coordinates_thread = threading.Thread(target = self.update_face_coordinates)
self.update_coordinates_thread.start()
print "Setting up connection.\n"
self.initialise_connection()
print "Initialising Motors..\n"
self.initialise_motors()
print "Awaiting new connection...\n"
self.update_face_coordinates()
def initialise_connection(self):
hostname = socket.gethostbyname( '0.0.0.0' )
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket.bind((hostname, self.PORT_NUMBER))
self.socket.listen(1)
def initialise_motors(self):
self.set_up_motor(self.right_track_motor)
self.set_up_motor(self.left_track_motor)
#set_up_motor(self.servo_arm_pitch_motor)
#set_up_motor(self.servo_arm_roll_motor)
def set_up_motor(self, motor):
motor.reset()
motor.position = self.MOTOR_STARTING_POSITION
motor.regulation_mode = self.DEFAULT_REGULATION_MODE
motor.pulses_per_second_sp = self.DEFAULT_PULSES_PER_SECOND
motor.hold_mode = self.DEFAULT_HOLD_MODE
motor.stop_mode = self.DEFAULT_STOP_MODE
def current_time(self):
return time.time()/1000 # In seconds
def set_face_coordinates(self, x, y):
with self.face_coordinates_lock:
self.x = x
self.y = y
def get_face_coordinates(self):
return self.x, self.y
def set_system_on(self, state):
with self.system_on_lock:
self.system_on = state
def get_system_on(self):
return self.system_on
def calculate_x_axis_pid(self):
print "time 1" + str(self.current_time())
face_x, face_y = self.get_face_coordinates()
dt = self.current_time() - self.previous_dt
error = face_x - self.offset
self.integral = self.integral + (error*dt)
self.derivative = (error - self.previous_err)/dt
turning_power = (self.kp*error) + (self.ki*self.integral) + (self.kd*self.derivative)
left_motor_power = self.tp + turning_power # These depend on the inital
right_motor_power = self.tp - turning_power # orientation of the motors
print str(turning_power)
self.right_track_motor.pulses_per_second_sp = right_motor_power
self.left_track_motor.pulses_per_second_sp = left_motor_power
self.previous_dt = dt
self.previous_err = error
print "time 2" + str(self.current_time())
def stop_x_axis_motors(self):
self.right_track_motor.stop()
self.left_track_motor.stop()
def start_x_axis_motors(self):
self.right_track_motor.start()
self.left_track_motor.start()
def motor_control(self):
self.previous_dt = self.current_time()
try:
if self.coordinates_time_recv > 0:
self.calculate_x_axis_pid()
self.start_x_axis_motors()
else:
self.previous_dt = time.time()
self.stop_x_axis_motors()
except (KeyboardInterrupt, SystemExit):
#self.cleanup()
raise
def update_face_coordinates(self):
conn, addr = self.socket.accept()
print conn, addr
self.coordinates_time_recv = -1
print "Connection address: ", addr
self.set_system_on(True)
while self.get_system_on():
try:
data = conn.recv(self.unpacker.size) #data size
if not data:
if self.current_time() > (self.coordinates_time_recv*self.timeout) and (self.coordinates_time_recv > -1):
self.coordinates_time_recv = -1
raise CoordinatesTimeOut()
else:
face_x, face_y = self.unpacker.unpack(data)
print face_x, face_y
self.coordinates_time_recv = self.current_time()
self.set_face_coordinates(face_x, face_y)
print "Face coordinates recieved at: " + str(self.coordinates_time_recv) + "s, Position: (" + str(face_x) + "," + str(face_y) + ")\n"
self.motor_control()
except CoordinatesTimeOut, e:
self.coordinates_time_recv = -1
self.stop_x_axis_motors()
print "ERROR: No new coordinates recieved!\nThe current coordinates are no longer valid, stopping motors!\n" + e
except KeyboardInterrupt, SystemExit:
self.cleanup()
conn.close()
