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
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()