def __init__(self, init_pose, soc=None, port_motor_rear=OUTPUT_A, port_motor_steer=OUTPUT_B, port_sensor_gyro='in1'): # initialization of devices self.__button_halt = Button() self.__motor_rear = LargeMotor(port_motor_rear) self.__motor_steer = LargeMotor(port_motor_steer) self.__sensor_gyro = GyroSensor(port_sensor_gyro) self.__velocity_controller = VelocityController(self, 0, 0, adaptation=False) self.s = soc # NOTE: possible using other controllers. Change only here! self.__trajectory_controller = ControllerWithLinearization() self.__path_controller = PathController() self.__localization = Localization(self, init_pose) self.__robot_state = [0, 0, 0, 0, 0, 0] # x,y, dx,dy, theta,omega self.reset() Sound.speak('Ready').wait()
def __init__(self, port_motor_rear=OUTPUT_A, port_motor_steer=OUTPUT_B, port_sensor_gyro='in1', port_sensor_us_front='in2', port_sensor_us_rear='in3'): # initialization of devices self.__button_halt = Button() self.__motor_rear = LargeMotor(port_motor_rear) self.__motor_steer = LargeMotor(port_motor_steer) self.__sensor_gyro = GyroSensor(port_sensor_gyro) self.__sensor_us_rear = UltrasonicSensor(port_sensor_us_rear) self.__sensor_us_front = UltrasonicSensor(port_sensor_us_front) self.__velocity_controller = VelocityController(self, 0, 0) # NOTE: possible using other controllers. Change only here! self.__trajectory_controller = ControllerWithLinearization() self.__localization = Localization(self) self.__robot_state = [0, 0, 0, 0, 0, 0] # x,y, dx,dy, theta,omega self.reset() Sound.speak('Initialization complete!').wait()
def __init__(self): self.left_color_sensor = ColorSensor('in4') self.right_color_sensor = ColorSensor('in1') self.left_large_motor = LargeMotor('outD') self.right_large_motor = LargeMotor('outA') self.touch_sensor = TouchSensor('in3') self.listeners = [] self.prev_is_pressed = False
def init_large_motor(port: str) -> LargeMotor: """ Preveri, ali je motor priklopljen na izhod `port`. Vrne objekt za motor (LargeMotor). """ motor = LargeMotor(port) while not motor.connected: print('\nPriklopi motor na izhod ' + port + ' in pritisni + spusti gumb DOL.') wait_for_button('down') motor = LargeMotor(port) return motor
def __init__(self, left_motor_port: str = OUTPUT_B, right_motor_port: str = OUTPUT_C, polarity: str = Motor.POLARITY_NORMAL, ir_sensor_port: str = INPUT_4, ir_beacon_channel: int = 1): self.left_motor = LargeMotor(address=left_motor_port) self.right_motor = LargeMotor(address=right_motor_port) self.left_motor.polarity = self.right_motor.polarity = polarity self.ir_sensor = InfraredSensor(address=ir_sensor_port) self.tank_drive_remote_control = \ RemoteControl( sensor=self.ir_sensor, channel=ir_beacon_channel)
def __init__(self, motor1Port, motor2Port, gyroPort=None, wheelDiameter=None): #init if GyroSensor != None: self.GS = GyroSensor(gyroPort) else: self.GS = GyroSensor() self.M1 = LargeMotor(motor1Port) self.M2 = LargeMotor(motor2Port) self.motor_stop = True self.wheelDiameter = wheelDiameter self.time = 0 self.MDistanceRunning = True self.distance = 0 self.pauseDistance = []
def __init__( self, back_foot_motor_port: str = OUTPUT_C, front_foot_motor_port: str = OUTPUT_B, gear_motor_port: str = OUTPUT_A, touch_sensor_port: str = INPUT_1, color_sensor_port: str = INPUT_3, ir_sensor_port: str = INPUT_4, ir_beacon_channel: int = 1): self.front_foot_motor = LargeMotor(address=front_foot_motor_port) self.back_foot_motor = LargeMotor(address=back_foot_motor_port) self.gear_motor = MediumMotor(address=gear_motor_port) self.touch_sensor = TouchSensor(address=touch_sensor_port) self.color_sensor = ColorSensor(address=color_sensor_port) self.ir_sensor = InfraredSensor(address=ir_sensor_port) self.remote_control = RemoteControl(sensor=self.ir_sensor, channel=ir_beacon_channel)
def __init__(self, left_foot_track_motor_port: str = OUTPUT_B, right_foot_track_motor_port: str = OUTPUT_C, bazooka_blast_motor_port: str = OUTPUT_A, touch_sensor_port: str = INPUT_1, color_sensor_port: str = INPUT_3, ir_sensor_port: str = INPUT_4, driving_ir_beacon_channel: int = 1, shooting_ir_beacon_channel: int = 2): self.left_foot_track_motor = LargeMotor( address=left_foot_track_motor_port) self.right_foot_track_motor = LargeMotor( address=right_foot_track_motor_port) self.bazooka_blast_motor = MediumMotor( address=bazooka_blast_motor_port) self.ir_sensor = InfraredSensor(address=ir_sensor_port) self.driving_remote_control = RemoteControl( sensor=self.ir_sensor, channel=driving_ir_beacon_channel) self.shooting_remote_control = RemoteControl( sensor=self.ir_sensor, channel=shooting_ir_beacon_channel) self.touch_sensor = TouchSensor(address=INPUT_1) self.color_sensor = ColorSensor(address=INPUT_3) self.leds = Leds() self.screen = Screen() assert self.left_foot_track_motor.connected assert self.right_foot_track_motor.connected assert self.bazooka_blast_motor.connected assert self.ir_sensor.connected assert self.touch_sensor.connected assert self.color_sensor.connected # reset the motors for motor in (self.left_foot_track_motor, self.right_foot_track_motor, self.bazooka_blast_motor): motor.reset() motor.position = 0 motor.stop_action = Motor.STOP_ACTION_BRAKE self.draw_face()
def __init__(self, left_motor_port: str = OUTPUT_B, right_motor_port: str = OUTPUT_C, grip_motor_port: str = OUTPUT_A, touch_sensor_port: str = INPUT_1, ir_sensor_port: str = INPUT_4, ir_beacon_channel: int = 1): self.left_motor = LargeMotor(address=left_motor_port) self.right_motor = LargeMotor(address=right_motor_port) self.grip_motor = MediumMotor(address=grip_motor_port) self.touch_sensor = TouchSensor(address=touch_sensor_port) self.ir_sensor = InfraredSensor(address=ir_sensor_port) self.ir_beacon_channel = ir_beacon_channel self.beacon = RemoteControl(sensor=self.ir_sensor, channel=ir_beacon_channel) self.speaker = Sound()
def __init__( self, left_foot_motor_port: str = OUTPUT_B, right_foot_motor_port: str = OUTPUT_C, bazooka_blast_motor_port: str = OUTPUT_A, touch_sensor_port: str = INPUT_1, color_sensor_port: str = INPUT_3, ir_sensor_port: str = INPUT_4, ir_beacon_channel: int = 1): self.left_foot_motor = LargeMotor(address=left_foot_motor_port) self.right_foot_motor = LargeMotor(address=right_foot_motor_port) self.bazooka_blast_motor = MediumMotor(address=bazooka_blast_motor_port) self.touch_sensor = TouchSensor(address=touch_sensor_port) self.color_sensor = ColorSensor(address=color_sensor_port) self.ir_sensor = InfraredSensor(address=ir_sensor_port) self.remote_control = RemoteControl(sensor=self.ir_sensor, channel=ir_beacon_channel) self.screen = Screen() self.speaker = Sound()
def __init__(self, turn_motor_port: str = OUTPUT_A, move_motor_port: str = OUTPUT_B, scare_motor_port: str = OUTPUT_D, touch_sensor_port: str = INPUT_1, color_sensor_port: str = INPUT_3, ir_sensor_port: str = INPUT_4, ir_beacon_channel: int = 1): self.turn_motor = MediumMotor(address=turn_motor_port) self.move_motor = LargeMotor(address=move_motor_port) self.scare_motor = LargeMotor(address=scare_motor_port) self.touch_sensor = TouchSensor(address=touch_sensor_port) self.color_sensor = ColorSensor(address=color_sensor_port) self.ir_sensor = InfraredSensor(address=ir_sensor_port) self.beacon = RemoteControl(sensor=self.ir_sensor, channel=ir_beacon_channel) self.noise = Sound()
def __init__(self, left_motor_port, right_motor_port, front_us_port, right_us_port, left_us_port): self.leftMotor = LargeMotor('out' + left_motor_port) self.rightMotor = LargeMotor('out' + right_motor_port) self.FRONT_US_SENSOR = UltrasonicSensor('in' + front_us_port) self.RIGHT_US_SENSOR = UltrasonicSensor('in' + right_us_port) self.LEFT_US_SENSOR = UltrasonicSensor('in' + left_us_port) self.TOUCH_SENSOR = TouchSensor() assert self.leftMotor.connected, "Connect left Motor to port" + \ str(left_motor_port) assert self.rightMotor.connected, "Connect right Motor to port" + \ str(right_motor_port) assert self.TOUCH_SENSOR.connected, "Connect a touch sensor" assert self.FRONT_US_SENSOR.connected, "Connect the ultrasound sensor in the front" assert self.RIGHT_US_SENSOR.connected, "Connect the ultrasound sensor on the right" assert self.LEFT_US_SENSOR.connected, "Connect the ultrasound sensor on the left" #set sensor mode to cm self.FRONT_US_SENSOR.mode = 'US-DIST-CM' self.RIGHT_US_SENSOR.mode = 'US-DIST-CM' self.LEFT_US_SENSOR.mode = 'US-DIST-CM'
def __init__(self, sting_motor_port: str = OUTPUT_D, go_motor_port: str = OUTPUT_B, claw_motor_port: str = OUTPUT_A, touch_sensor_port: str = INPUT_1, color_sensor_port: str = INPUT_3, ir_sensor_port: str = INPUT_4, ir_beacon_channel: int = 1): self.sting_motor = LargeMotor(address=sting_motor_port) self.go_motor = LargeMotor(address=go_motor_port) self.claw_motor = MediumMotor(address=claw_motor_port) self.ir_sensor = InfraredSensor(address=ir_sensor_port) self.ir_beacon_channel = ir_beacon_channel self.beacon = RemoteControl(sensor=self.ir_sensor, channel=self.ir_beacon_channel) self.touch_sensor = TouchSensor(address=touch_sensor_port) self.color_sensor = ColorSensor(address=color_sensor_port) self.dis = Screen() self.speaker = Sound()
#!/usr/bin/env python3 from ev3dev.ev3 import (Motor, LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_B, OUTPUT_D, InfraredSensor, INPUT_4, Leds, Sound) from random import randint from time import sleep MEDIUM_MOTOR = MediumMotor(address=OUTPUT_A) TAIL_MOTOR = LargeMotor(address=OUTPUT_B) CHEST_MOTOR = LargeMotor(address=OUTPUT_D) IR_SENSOR = InfraredSensor(address=INPUT_4) LIGHTS = Leds() SPEAKER = Sound() CHEST_MOTOR.run_timed(speed_sp=-300, time_sp=1000, stop_action=Motor.STOP_ACTION_BRAKE) CHEST_MOTOR.wait_while(Motor.STATE_RUNNING) while True: if IR_SENSOR.proximity < 30: LIGHTS.set_color(group=Leds.LEFT, color=Leds.RED, pct=1) LIGHTS.set_color(group=Leds.RIGHT, color=Leds.RED, pct=1) MEDIUM_MOTOR.stop(stop_action=Motor.STOP_ACTION_HOLD) TAIL_MOTOR.stop(stop_action=Motor.STOP_ACTION_BRAKE)
from ev3dev.ev3 import LargeMotor m1 = LargeMotor('outC') m2 = LargeMotor('outD') m1.stop() m2.stop()
def run(fun): # Connect two motors and two (different) light sensors mL = LargeMotor('outC') mR = LargeMotor('outB') sL = ColorSensor('in1') sR = ColorSensor('in4') gy = GyroSensor('in3') # Check if the sensors are connected assert sL.connected, "Left ColorSensor is not connected" assert sR.connected, "Right ColorSensor is not conected" assert gy.connected, "Gyro is not connected" gyro = Gyro() light_sensors = LightSensors() encoder = Encoder() # Set the motor mode mL.polarity = "normal" # "inversed" mR.polarity = "normal" # "inversed" mL.position = 0 mR.position = 0 def stop_motors(): mL.run_direct() mR.run_direct() mL.duty_cycle_sp = 0 mR.duty_cycle_sp = 0 stop_motors() # The example doesn't end on its own. # Use CTRL-C to exit it (needs command line). # This is a generic way to be informed # of this event and then take action. def signal_handler(sig, frame): stop_motors() print('Shut down gracefully') exit(0) # Install the signal handler for CTRL+C signal(SIGINT, signal_handler) print('Press Ctrl+C to exit') per = { 'mL': mL, 'mR': mR, 'sL': sL, 'sR': sR, 'gy': gy } light_sensors.init(per) gyro.init(gy) # Endless loop reading sensors and controlling motors. last_log = time() last_now = time() max_dt = 0 dts = 0 speed_mL = None speed_mR = None while True: state = {} state = light_sensors(per, state) state = encoder(per, state) state = gyro(per, state) state = fun(per, state) max_speed = 40 * TICKS_PER_CM _speed_mL = state.get('mL', 0) if _speed_mL != speed_mL: speed_mL = _speed_mL mL.run_forever(speed_sp=speed_mL/100 * max_speed) _speed_mR = state.get('mR', 0) if _speed_mR != speed_mR: speed_mR = _speed_mR mR.run_forever(speed_sp=speed_mR/100 * max_speed) dts += 1 now = time() dt = now - last_now last_now = now if dt > max_dt: max_dt = dt if now - last_log > 5.0: last_log = now print("avg fps: ", dts / 5.0) print('min fps: ', 1 / max_dt) max_dt = 0 dts = 0
def main(): data = None if True: with open('zemljevid.json') as f: data = json.load(f) else: resource = urllib.request.urlopen( 'http://192.168.0.200:8080/zemljevid.json') content = resource.read() content = content.decode("utf-8") data = json.loads(content) os.system('setfont Lat15-TerminusBold14') if os.path.exists("logs"): import shutil shutil.rmtree("logs") os.mkdir("logs") mL = LargeMotor('outB') mL.stop_action = 'hold' mR = LargeMotor('outC') mR.stop_action = 'hold' global cl cl = ColorSensor() cl.mode = 'COL-COLOR' gy = GyroSensor() gy.mode = 'GYRO-RATE' gy.mode = 'GYRO-ANG' # Give gyro a bit of time to start time.sleep(3) global start_time start_time = time.time() debug_print("Start time: ", start_time) start = [0, 0] locations = [] for key, item in data.items(): if key == "start": start = item else: locations.append(item) # Sort by distance, TODO might be better to minimize turns by prioritizing victims that are in the same line locations = sorted( locations, key=lambda x: abs(start[0] - x[0]) + abs(start[1] - x[1]), reverse=False) current_location = start def reset_neighbourhood_search(): global searching_neighbourhood nonlocal neighbourhood_locations searching_neighbourhood = False neighbourhood_locations = [] global searching_neighbourhood searching_neighbourhood = False neighbourhood_locations = [] while locations: next_location = locations.pop(0) go_to_location(x=next_location[0], y=next_location[1], current_x=current_location[0], current_y=current_location[1], mL=mL, mR=mR, gyro=gy) current_location = next_location color = cl.value() if color == 1: # BLACK: START, 2 second beep debug_print("Color sensor: START") beep(1, 2000) reset_neighbourhood_search() locations = sorted( locations, key=lambda x: abs(start[0] - x[0]) + abs(start[1] - x[1]), reverse=False) elif color == 2: # BLUE: good condition, 1 beep debug_print("Color sensor: BLUE") beep(1) reset_neighbourhood_search() locations.insert(0, start) elif color == 4: # YELLOW: critical condition, 2 beeps debug_print("Color sensor: YELLOW") beep(2) reset_neighbourhood_search() locations.insert(0, start) elif color == 5: # RED: passed away, 3 beeps debug_print("Color sensor: RED") beep(3) reset_neighbourhood_search() #locations.insert(0, start) locations = sorted(locations, key=lambda x: abs(current_location[0] - x[0]) + abs(current_location[1] - x[1]), reverse=False) else: debug_print("Color sensor: UNKNOWN (" + str(color) + ")") #locations.insert(0, start) if not searching_neighbourhood: searching_neighbourhood = True radius = 5 for area in range(1, 20): neighbourhood_locations.append([ next_location[0] + radius * area, next_location[1] - radius * area ]) neighbourhood_locations.append([ next_location[0] + radius * area, next_location[1] + radius * area ]) neighbourhood_locations.append([ next_location[0] - radius * area, next_location[1] + radius * area ]) neighbourhood_locations.append([ next_location[0] - radius * area, next_location[1] - radius * area ]) locations.insert(0, neighbourhood_locations[0]) neighbourhood_locations = neighbourhood_locations[1:] # # Rotate back to original orientation # angle = calculate_angle(0, gy.value()) # rotate_to_angle(angle, mL, mR, gy) # for i in range (16): # rotate_to_angle(90, mL, mR, gy) # rotate_to_angle(0, mL, mR, gy) #for _ in range (5): #rotate_to_angle(89, mL, mR, gy) #rotate_to_angle(178, mL, mR, gy) #rotate_to_angle(270, mL, mR, gy) #rotate_to_angle(180, mL, mR, gy) #rotate_to_angle(90, mL, mR, gy) #rotate_to_angle(0, mL, mR, gy) #rotate_to_angle(-91, mL, mR, gy) #rotate_to_angle(-182, mL, mR, gy) #rotate_to_angle(-2, mL, mR, gy) #rotate_to_angle(89, mL, mR, gy) #rotate_to_angle(178, mL, mR, gy) #rotate_to_angle(-2, mL, mR, gy) #rotate_to_angle(87, mL, mR, gy) #rotate_to_angle(176, mL, mR, gy) # for i in range (5): # drive_for_centimeters(50, mL, mR, gy, 0) # rotate_to_angle(0, mL, mR, gy) # drive_for_centimeters(-50, mL, mR, gy, 0) # rotate_to_angle(0, mL, mR, gy) debug_print("End time: ", time.time())
#!/usr/bin/env python3 from ev3dev.ev3 import LargeMotor, ColorSensor, UltrasonicSensor, InfraredSensor, MediumMotor, INPUT_1, INPUT_2, INPUT_3, INPUT_4 # MOTORES esq = LargeMotor('outA') dir = LargeMotor('outB') motor_garra = LargeMotor('outC') #motor_sensor = MediumMotor('outD') # SENSORES sensor_esq = ColorSensor(address=INPUT_1) sensor_dir = ColorSensor(address=INPUT_2) sensor_frente = UltrasonicSensor(address=INPUT_3) sensor_lado = UltrasonicSensor(address=INPUT_4) sensor_esq.mode = 'COL-REFLECT' sensor_dir.mode = 'COL-REFLECT'
#!/usr/bin/env python3 from ev3dev.ev3 import LargeMotor, ColorSensor, GyroSensor, InfraredSensor, Button from Classes.Motores import * from Classes.PID import * from time import time, sleep from os import system import paho.mqtt.client as mqtt # Motores M_PORTA = LargeMotor("outC") M_ESQUERDO = LargeMotor("outA") M_DIREITO = LargeMotor("outB") # Sensores infravermelhos PROX1 = InfraredSensor("in4") PROX2 = InfraredSensor("in3") PROX1.mode = "IR-PROX" PROX2.mode = "IR-PROX" # Giroscopio GYRO = GyroSensor("in1") GYRO.mode = "GYRO-ANG" # Sensor de cor COLOR = ColorSensor("in2") COLOR.mode = "COL-COLOR" # Variaveis usadas durante o programa # Constantes TEMPO_CURVA_90 = 1700
#!/usr/bin/env python3 from time import sleep from ev3dev.ev3 import LargeMotor, GyroSensor, UltrasonicSensor, ColorSensor from os import system system('setfont Lat15-TerminusBold14') cl_left = ColorSensor('in1') cl_right = ColorSensor('in2') l = LargeMotor('outB') r = LargeMotor('outA') gyro = GyroSensor('in3') sonic = UltrasonicSensor('in4') def girar(graus): pos0 = gyro.value() if (graus > 0): while gyro.value() < pos0 + graus: l.run_forever(speed_sp=-500) r.run_forever(speed_sp=250) else: while gyro.value() > pos0 + graus: l.run_forever(speed_sp=250) r.run_forever(speed_sp=-500) l.stop() r.stop()
#!/usr/bin/env python3 from ev3dev.ev3 import ( Motor, MediumMotor, LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_D, InfraredSensor, INPUT_4, Sound ) MEDIUM_MOTOR = MediumMotor(address=OUTPUT_A) GO_MOTOR = LargeMotor(address=OUTPUT_B) STING_MOTOR = LargeMotor(address=OUTPUT_D) INFRARED_SENSOR = InfraredSensor(address=INPUT_4) SPEAKER = Sound() MEDIUM_MOTOR.run_timed( speed_sp=500, time_sp=1000, stop_action=Motor.STOP_ACTION_HOLD) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING) MEDIUM_MOTOR.run_timed( speed_sp=-500, time_sp=0.3 * 1000, stop_action=Motor.STOP_ACTION_HOLD) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING)
#!/usr/bin/python from ev3dev.ev3 import LargeMotor as LargeMotor from ev3dev.ev3 import TouchSensor as TouchSensor from time import sleep power = -100 run_time = 3 motor_b = LargeMotor(address='outB') motor_c = LargeMotor(address='outC') motor_b.reset() motor_c.reset() motor_b.command = 'run-direct' motor_c.command = 'run-direct' trigger = TouchSensor() print("Fire when ready!") while True: if trigger.value() == True: break else: sleep(.01) motor_b.duty_cycle_sp = power motor_c.duty_cycle_sp = power
import time from ev3dev.ev3 import LargeMotor, ColorSensor, UltrasonicSensor, Sound, GyroSensor from enum import Enum, IntEnum from pickled import pickled from ring import RingBuf from util import SubinstructionError # -- MOTORS -- mLeft = LargeMotor('outD') mRight = LargeMotor('outC') mRight.polarity = 'inversed' mLeft.polarity = 'inversed' # -- SENSORS -- sSonic = UltrasonicSensor('in1') sSonic.mode = UltrasonicSensor.MODE_US_DIST_CM sGyro = GyroSensor('in2') sGyro.mode = GyroSensor.MODE_GYRO_ANG cLeft = ColorSensor('in3') cRight = ColorSensor('in4') """ Marks which side of the robot the line is on. """ class MoveDir(IntEnum): LINE_LEFT = -1 LINE_RIGHT = 1
#!/usr/bin/env python3 from time import sleep from ev3dev.ev3 import LargeMotor, GyroSensor, UltrasonicSensor, ColorSensor, Sound from os import system system('setfont Lat15-TerminusBold14') cl_left = ColorSensor('in1') cl_right = ColorSensor('in4') l = LargeMotor('outA') r = LargeMotor('outC') gyro = GyroSensor('in2') sonic = UltrasonicSensor('in3') gradient = [] count = 0 while 1: l.run_forever(speed_sp=-300) r.run_forever(speed_sp=-300) count += 1 if(count % 25 == 0): count = 1 gradient.append((cl_left.value(), cl_right.value())) if(len(gradient) > 10):
#!/usr/bin/env python3 from ev3dev.ev3 import ( Motor, LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, TouchSensor, InfraredSensor, RemoteControl, INPUT_1, INPUT_4, Sound ) MEDIUM_MOTOR = MediumMotor(address=OUTPUT_A) LEFT_MOTOR = LargeMotor(address=OUTPUT_B) RIGHT_MOTOR = LargeMotor(address=OUTPUT_C) TOUCH_SENSOR = TouchSensor(address=INPUT_1) IR_SENSOR = InfraredSensor(address=INPUT_4) BEACON_CONTROL = RemoteControl(sensor=IR_SENSOR, channel=1) SPEAKER = Sound() def drive_once_by_ir_beacon(speed: float = 1000): if BEACON_CONTROL.red_up and BEACON_CONTROL.blue_up: # go forward LEFT_MOTOR.run_forever(speed_sp=speed) RIGHT_MOTOR.run_forever(speed_sp=speed) elif BEACON_CONTROL.red_down and BEACON_CONTROL.blue_down: # go backward LEFT_MOTOR.run_forever(speed_sp=-speed)
#!/usr/bin/env python from ev3dev.ev3 import LargeMotor, \ OUTPUT_A, Sound, PowerSupply from time import time, sleep from math import pi, copysign N = 1000 file_name = "data_pc.txt" Kp = 20 pwr = 0 motor = LargeMotor(OUTPUT_A) battary = PowerSupply() Sound().beep() fout = open(file_name, "w") sleep(0.05) motor.reset() print(motor.position) start_time = time() for i in range(0, N): t = time() - start_time rotation = motor.position e = (360 - rotation) * pi / 180 pwr = Kp * e pwr = pwr / battary.measured_volts * 100 if abs(pwr) > 100: pwr = copysign(1, pwr) * 100 motor.run_direct(duty_cycle_sp=pwr) line = "%f\t%d\n" % (t, rotation) fout.write(line)
#!/usr/bin/env python3 from ev3dev.ev3 import ColorSensor, INPUT_1, INPUT_2, OUTPUT_A, OUTPUT_B, LargeMotor, Button, GyroSensor, Sound, UltrasonicSensor from PID import PID from os import system from time import sleep from json import dump, load # Instanciando sensores cl_left = ColorSensor(address=INPUT_1) cl_right = ColorSensor(address=INPUT_2) gyro = GyroSensor('in3') sonic = UltrasonicSensor('in4') # Instanciando motores m_left = LargeMotor(address=OUTPUT_A) m_right = LargeMotor(address=OUTPUT_B) # Verificando se os sensores/motores estão conectados assert cl_left.connected assert cl_right.connected assert gyro.connected assert sonic.connected assert m_left.connected assert m_right.connected # Definindo modo reflectância cl_left.mode = 'COL-REFLECT' cl_right.mode = 'COL-REFLECT' gyro.mode = 'GYRO-ANG'
WHEEL_DIAMETER = 4.3 # centimeters WHEEL_CIRCUMFERENCE = pi * WHEEL_DIAMETER ROTATIONS_PER_CM = 360 * WHEEL_CIRCUMFERENCE # TODO ROTATIONS_PER_DEGREE needs a real number, .5 is just a wild guess ROTATIONS_PER_DEGREE = .5 # how many rotations to turn specified degrees """ 0 | -90/270 ---|--- 90/-270 | 180/-180 """ DIRECTION = 0 LEFT_MOTOR = LargeMotor(OUTPUT_B) RIGHT_MOTOR = LargeMotor(OUTPUT_C) MEDIUM_MOTOR = MediumMotor(OUTPUT_A) TOUCH_SENSOR = TouchSensor() IR_SENSOR = InfraredSensor() SCREEN = Screen() LEDS = Leds SILENT = False SP_FOR_90_DEG_TURN = 295 def set_silent(silent=False): global SILENT SILENT = silent
#!/usr/bin/env python3 from ev3dev.ev3 import (Motor, LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_B, Sound) MEDIUM_MOTOR = MediumMotor(address=OUTPUT_A) GO_MOTOR = LargeMotor(address=OUTPUT_B) SPEAKER = Sound() MEDIUM_MOTOR.run_timed(speed_sp=500, time_sp=1000, stop_action=Motor.STOP_ACTION_HOLD) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING) MEDIUM_MOTOR.run_timed(speed_sp=-500, time_sp=0.3 * 1000, stop_action=Motor.STOP_ACTION_HOLD) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING) for i in range(2): for p in range(3): MEDIUM_MOTOR.run_timed(speed_sp=750, time_sp=0.2 * 1000, stop_action=Motor.STOP_ACTION_HOLD) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING) MEDIUM_MOTOR.run_timed(speed_sp=-750, time_sp=0.2 * 1000, stop_action=Motor.STOP_ACTION_HOLD)
#!/usr/bin/env python3 from ev3dev.ev3 import (Motor, LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_D, INPUT_4, InfraredSensor, Sound) from threading import Thread from time import sleep LARGE_MOTOR = LargeMotor(address=OUTPUT_D) MEDIUM_MOTOR = MediumMotor(address=OUTPUT_A) IR_SENSOR = InfraredSensor(address=INPUT_4) SPEAKER = Sound() def rattle(): while True: MEDIUM_MOTOR.run_timed(speed_sp=100, time_sp=1000, stop_action=Motor.STOP_ACTION_COAST) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING) SPEAKER.play(wav_file='/home/robot/sound/Snake rattle.wav').wait() MEDIUM_MOTOR.run_timed(speed_sp=-100, time_sp=1000, stop_action=Motor.STOP_ACTION_COAST) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING) sleep(1)