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