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 turnWheel(self, phi_desired): """ Turns front wheels on the desired angle 'phi_desired' """ pid_phi = PID(100.0, 500.0, 5.0, 100, -100) t = 0 clock = Clock() while t < 1: # FIXME: seems that need steady state condition t, dt = clock.getTandDT() phi_current = self.__motor_steer.position error_phi = radians(phi_desired - phi_current) u_phi = pid_phi.getControl(error_phi, dt) self.__motor_steer.run_direct(duty_cycle_sp=u_phi) Sound.speak('Wheels were turned!')
def all_green(): process = robot.scan(200) g_count = 0 for i, j in process: if i == Color.GREEN: g_count += 1 if g_count == len(process): Leds.set_color(Leds.RIGHT, Leds.GREEN) Leds.set_color(Leds.LEFT, Leds.GREEN) Sound.speak("good to go, all green").wait() Leds.all_off() return True else: return False
def speak(text, wait=False): process = _Sound.speak(text) if wait: process.wait()
A long time ago in a galaxy far, far away... """)) Sound.play_song(( ('D4', 'e3'), ('D4', 'e3'), ('D4', 'e3'), ('G4', 'h'), ('D5', 'h'), ('C5', 'e3'), ('B4', 'e3'), ('A4', 'e3'), ('G5', 'h'), ('D5', 'q'), ('C5', 'e3'), ('B4', 'e3'), ('A4', 'e3'), ('G5', 'h'), ('D5', 'q'), ('C5', 'e3'), ('B4', 'e3'), ('C5', 'e3'), ('A4', 'h.'), )).wait() Sound.play(os.path.join(_HERE, 'snd/r2d2.wav')).wait() Sound.speak("Luke, I am your father").wait()
#turn 180 in place from ev3dev import ev3 from ev3dev.ev3 import Sound import robotFunctions toTurn = 180 move = robotFunctions.mover('outA', 'outD') gy = ev3.GyroSensor() gy.mode = 'GYRO-CAL' gy.mode = 'GYRO-ANG' start = gy.value() speed = 200 turnRight = 'right' move.drive(0, toTurn, turnRight, speed) end = gy.value() turnCon = end - start units = gy.units conclusion = 'expected to turn {} degrees and turned {} {}'.format( toTurn, turnCon, units) print(conclusion) Sound.speak(conclusion).wait()
def handleMessage(message): angle_coef = 20 global readyToRide # print('delivered : '+ str(message[0])) if message[0] == 254: return if message[0] == 255: mid_motor.stop() if message[0] % 4 == 1: Leds.set_color(Leds.RIGHT, Leds.RED) Leds.set_color(Leds.LEFT, Leds.GREEN) left_motor.run_forever(speed_sp=-1000) if (min((message[0] - 128) // 4 * angle_coef, 1000) < -800): right_motor.run_forever(speed_sp=min((message[0] - 128) // 4 * angle_coef, 1000)) else: right_motor.run_forever(speed_sp=-1000) print(-1000, min(-1000 + message[0] // 4 * angle_coef, 1000)) # Sound.speak('RUN') # print("a") Sound.speak('Stop') print("b") print("ya loh") Sound.speak('HAHA PAPA CARLO YA OBMANUL TEBIA') print("ya loh1") sleep(5) print("ya loh2") if message[0] % 4 == 0: Leds.set_color(Leds.RIGHT, Leds.YELLOW) right_motor.stop() left_motor.stop() Sound.speak('Stop') print("b") print("ya loh") Sound.speak('HAHA PAPA CARLO YA OBMANUL TEBIA') print("ya loh1") sleep(5) print("ya loh2") if message[0] % 4 == 2: Leds.set_color(Leds.LEFT, Leds.RED) Leds.set_color(Leds.RIGHT, Leds.GREEN) if (min((message[0] - 128) // 4 * angle_coef, 1000) < -800): left_motor.run_forever(speed_sp=min((message[0] - 128) // 4 * angle_coef, 1000)) else: left_motor.run_forever(speed_sp=-1000) right_motor.run_forever(speed_sp=-1000) print(min(-1000 + message[0] // 4 * angle_coef, 1000), -1000) Sound.speak('Stop') print("b") print("ya loh") Sound.speak('HAHA PAPA CARLO YA OBMANUL TEBIA') print("ya loh1") sleep(5) print("ya loh2") if message[0] % 4 == 3: readyToRide = True left_motor.run_forever(speed_sp=-1000) right_motor.run_forever(speed_sp=-1000)
def handler(ip): print("STARTING") print("POSITION: %s" % last_pos.string) try: print("CONNECTING: %s:8000/" % ip) websocket = yield from websockets.connect("ws://%s:8000/" % ip) except OSError: print("Cannot connect to the server") return while True: try: status = {"status": "Retrieve Map"} yield from websocket.send(json.dumps(status)) print("> {}".format(status)) map_raw = yield from websocket.recv() print("< {}".format(map_raw)) map_obj = json.loads(map_raw) global bases global optimal_routes global endpoint_junction_connection global junction_endpoints bases = set(map_obj["bases"]) optimal_routes = map_obj["optimal_routes"] endpoint_junction_connection = map_obj[ "endpoint_junction_connection"] junction_endpoints = map_obj["junction_endpoints"] status = {"status": "Requesting new instruction"} yield from websocket.send(json.dumps(status)) print() print("> {}".format(status)) instruction_raw = yield from websocket.recv() print("< {}".format(instruction_raw)) if instruction_raw == "close": break else: instruction = json.loads(instruction_raw) cancelled = False # Generator that yields current position and is sending the cancellation gen = action_caller(instruction) try: # for new_position in gen: new_position, totalInstructions, currentInstruction = next( gen) except alreadyInPlaceException: continue except SubinstructionError as e: message = e.__str__() yield from subinstructionErrorHandler(message, websocket) while True: try: status = { "status": "Position and queue progress update", "position": new_position, # String "progress": { "currentSteps": currentInstruction, "totalSteps": totalInstructions # Integers } } yield from websocket.send(json.dumps(status)) print("> {}".format(status)) if not cancelled: status = {"status": "Check Cancellation"} yield from websocket.send(json.dumps(status)) print("> {}".format(status)) cancel_instruction_raw = yield from websocket.recv( ) print("< {}".format(cancel_instruction_raw)) cancel_instruction = json.loads( cancel_instruction_raw) cancelled = cancel_instruction["cancelled"] # Continue the operation try: new_position, totalInstructions, currentInstruction = gen.send( cancelled) except SubinstructionError as e: message = e.__str__() yield from subinstructionErrorHandler( message, websocket) except StopIteration: if not cancelled: break else: # Just keep updating position until cancelation complete try: status = { "status": "Position update", "position": new_position # String } yield from websocket.send(json.dumps(status)) print("> {}".format(status)) # Continue the operation try: new_position, totalInstructions, currentInstruction = gen.send( cancelled) except SubinstructionError as e: message = e.__str__() yield from subinstructionErrorHandler( message, websocket) except StopIteration: break # No need to send confirmation of instruction completed or cancelled except websockets.exceptions.ConnectionClosed: print("Lost connection to server") Sound.speak("Lost connection to server") break yield from websocket.close()
#drive continuesly on white page till blue line from ev3dev import ev3 from ev3dev.ev3 import Sound import robotFunctions move = robotFunctions.mover('outA', 'outD') #ar = ev3.MediumMotor('outB') cl = ev3.ColorSensor() cl.mode = 'RGB-RAW' speed = 200 #ar.run_to_abs_pos(position_sp = 0) while True: col = robotFunctions.get_closest_color([cl.value(i) for i in range(3)]) if col == 'white': move.drive(9.5, 0, '', speed) elif col == robotFunctions.Color.BLUE: break else: Sound.speak('error').wait() Sound.beep()