def cmd_waitdeg_target(deg, waitport, waitdeg, tarvar):
return cmd_tacho(waitport, tarvar) + b''.join([
ev3.opAdd32,
ev3.GVX(tarvar),
ev3.LCX(waitdeg if deg > 0 else -waitdeg),
ev3.GVX(tarvar)
])
def stop() -> None:
ops_read = b''.join([
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0), # LAYER
ev3.LCX(2), # NO
ev3.LCX(125), # TYPE - EV3-Touch
ev3.LCX(0), # MODE - Touch
ev3.LCX(1), # VALUES
ev3.GVX(0), # VALUE1
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0), # LAYER
ev3.LCX(2), # NO
ev3.LCX(125), # TYPE - EV3-Touch
ev3.LCX(1), # MODE - Bump
ev3.LCX(1), # VALUES
ev3.GVX(4) # VALUE1
])
ops_sound = play_sound(10, 200, 100)
reply = my_ev3.send_direct_cmd(ops_sound + ops_read, global_mem=8)
(r, c) = struct.unpack('<ff', reply[5:])
# if touched == 1:
# bumps += 0.5
print("color = ", c)
print("refect = ", r)
def cmd_waitdeg_wait(deg, waitport, tarvar, waitvar):
return cmd_tacho(waitport, waitvar) + b''.join([
ev3.opJr_Lt32 if deg > 0 else ev3.opJr_Gt32,
ev3.GVX(waitvar),
ev3.GVX(tarvar),
ev3.LCX(-9)
])
def gettouch(port) -> int:
global myEV3, stdscr
ops_read = b''.join([
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0), # LAYER
ev3.LCX(port), # NO
ev3.LCX(16), # TYPE - EV3-Touch
ev3.LCX(0), # MODE - Touch
ev3.LCX(1), # VALUES
ev3.GVX(0), # VALUE1
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0), # LAYER
ev3.LCX(1), # NO
ev3.LCX(16), # TYPE - EV3-Touch
ev3.LCX(1), # MODE - Bump
ev3.LCX(1), # VALUES
ev3.GVX(4) # VALUE1
])
# ops_sound = play_sound(10, 200, 100)
reply = myEV3.send_direct_cmd(ops_sound + ops_read, global_mem=8)
# return struct.unpack('<ff', reply[5:])
(touched, bumps) = struct.unpack('<ff', reply[5:])
if touched == 1:
bumps += 0.5
# print(bumps, "bumps")
return bumps
def remote_sensor_mode(port: int) -> None:
global my_ev3
ops = b''.join([
ev3.opInput_Device,
ev3.GET_TYPEMODE,
ev3.LCX(0), # LAYER
ev3.LCX(port), # NO
ev3.GVX(0), # TYPE
ev3.GVX(1) # MODE
])
reply = my_ev3.send_direct_cmd(ops, global_mem=2)
(type, mode) = struct.unpack('BB', reply[5:7])
print("Sensor Port: {} type: {}, mode: {}".format(port, type, mode))
def del_dir(self, path: str, secure: bool = True) -> None:
"""
Delete a directory on EV3's file system
Attributes:
path: absolute or relative path (from "/home/root/lms2012/sys/")
secure: flag, if the directory may be not empty
"""
if secure:
self.del_file(path)
else:
if path.endswith("/"):
path = path[:-1]
parent_path = path.rsplit("/", 1)[0] + "/"
folder = path.rsplit("/", 1)[1]
ops = b''.join([
ev3.opFile, ev3.GET_FOLDERS,
ev3.LCS(parent_path),
ev3.GVX(0)
])
reply = self.send_direct_cmd(ops, global_mem=1)
num = struct.unpack('B', reply[5:])[0]
found = False
for i in range(num):
ops = b''.join([
ev3.opFile,
ev3.GET_SUBFOLDER_NAME,
ev3.LCS(parent_path),
ev3.LCX(i + 1), # ITEM
ev3.LCX(64), # LENGTH
ev3.GVX(0) # NAME
])
reply = self.send_direct_cmd(ops, global_mem=64)
subdir = struct.unpack('64s', reply[5:])[0]
subdir = subdir.split(b'\x00')[0]
subdir = subdir.decode("utf8")
if subdir == folder:
found = True
ops = b''.join([
ev3.opFile,
ev3.DEL_SUBFOLDER,
ev3.LCS(parent_path), # NAME
ev3.LCX(i + 1) # ITEM
])
self.send_direct_cmd(ops)
break
if not found:
raise ev3.DirCmdError("Folder " + path + " doesn't exist")
def cmd_tacho(port, var):
return b''.join([
ev3.opInput_Device, ev3.GET_RAW,
ev3.LCX(0),
ev3.port_motor_input(port),
ev3.GVX(var)
])
def is_pressed(brick, port):
cmd = b''.join([
ev3.opInput_Read,
ev3.LCX(0),
ev3.LCX(port),
ev3.LCX(16),
ev3.LCX(0),
ev3.GVX(0)
])
return struct.unpack('<b',
brick.send_direct_cmd(cmd, global_mem=1)[5:])[0] > 0
def read_infrared(self) -> float:
ops = b''.join([
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0), # LAYER
ev3.LCX(3), # NO
ev3.LCX(33), # TYPE - EV3-IR
ev3.LCX(0), # MODE - Proximity
ev3.LCX(1), # VALUES
ev3.GVX(0) # VALUE1
])
reply = self.send_direct_cmd(ops, global_mem=4)
return struct.unpack('<f', reply[5:])[0]
def _ops_pos(self):
"""
read positions of the wheels (returns operations)
"""
return b''.join([
ev3.opInput_Device,
ev3.READY_RAW,
ev3.LCX(0), # LAYER
ev3.port_motor_input(self._port_left), # NO
ev3.LCX(7), # TYPE - EV3-Large-Motor
ev3.LCX(1), # MODE - Degree
ev3.LCX(1), # VALUES
ev3.GVX(0), # VALUE1
ev3.opInput_Device,
ev3.READY_RAW,
ev3.LCX(0), # LAYER
ev3.port_motor_input(self._port_right), # NO
ev3.LCX(7), # TYPE - EV3-Large-Motor
ev3.LCX(0), # MODE - Degree
ev3.LCX(1), # VALUES
ev3.GVX(4) # VALUE1
])
def distance(port) -> float:
global myEV3, stdscr
ops = b''.join([
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0), # LAYER
ev3.LCX(port), # NO
ev3.LCX(33), # TYPE - EV3-IR
ev3.LCX(0), # MODE - Proximity
ev3.LCX(1), # VALUES
ev3.GVX(0) # VALUE1
])
reply = myEV3.send_direct_cmd(ops, global_mem=4)
return struct.unpack('<f', reply[5:])[0]
def get_degrees_two_motors(self, motors):
ops = b''.join([
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0), # LAYER
ev3.port_motor_input(motors[0]['port']), # NO
ev3.LCX(7), # TYPE
ev3.LCX(0), # MODE
ev3.LCX(1), # VALUES
ev3.GVX(0), # VALUE1
ev3.opInput_Device,
ev3.READY_RAW,
ev3.LCX(0), # LAYER
ev3.port_motor_input(motors[1]['port']), # NO
ev3.LCX(7), # TYPE
ev3.LCX(0), # MODE
ev3.LCX(1), # VALUES
ev3.GVX(4) # VALUE1
])
reply = self.ev3.send_direct_cmd(ops, global_mem=8)
(pos_0, pos_1) = struct.unpack('<fi', reply[5:])
#pos_0 -= self.base_pos[0]
#pos_1 -= self.base_pos[1]
return pos_0, pos_1
def get_degree_single_motor(self, motors, motor_num):
ops = b''.join([
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0), # LAYER
ev3.port_motor_input(motors[motor_num]['port']), # NO
ev3.LCX(7), # TYPE
ev3.LCX(0), # MODE
ev3.LCX(1), # VALUES
ev3.GVX(0), # VALUE1
])
reply = self.ev3.send_direct_cmd(ops, global_mem=8)
pos = struct.unpack('<fi', reply[5:])
print(pos)
pos -= self.base_pos[motor_num]
return pos
def remote_gyro_input(port: int) -> int:
global my_ev3
ops = b''.join([
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0), # LAYER
ev3.LCX(port), # NO
ev3.LCX(32), # TYPE - EV3-IR
ev3.LCX(0), # MODE - Proximity
ev3.LCX(1), # VALUES
ev3.GVX(0) # VALUE1
])
reply = my_ev3.send_direct_cmd(ops, global_mem=4)
input = struct.unpack('<f', reply[5:9])[0]
print ("Remote GYRO input: {} in PORT: {}".format(input, port))
return input
def read_color(self) -> float:
ops = b''.join([
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0), # LAYER
ev3.LCX(0), # NO
ev3.LCX(4), # TYPE - EV3-IR
ev3.LCX(2), # MODE - Proximity
ev3.LCX(1), # VALUES
ev3.GVX(0) # VALUE1
])
reply = self.send_direct_cmd(ops, global_mem=4)
color = struct.unpack('<f', reply[5:])[0]
if color not in (2, 3, 4, 5):
color = 5
return color
def remote_motor_status(port: int) -> None:
ops = b''.join([
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0), # LAYER
ev3.port_motor_input(port), # NO
ev3.LCX(7), # TYPE
ev3.LCX(0), # MODE
ev3.LCX(1), # VALUES
ev3.GVX(0), # VALUE1
])
reply = my_ev3.send_direct_cmd(ops, global_mem=4)
(pos_a,) = struct.unpack('<f', reply[5:])
# pos_a = struct.unpack('<f', reply[5:])
print("Get Remote MOTOR in PORT with position {}".format(pos_a))
def ultra(port):
global lastUltra
global curUltra
if time() - lastUltra < 0.5:
return curUltra
ops = b''.join([
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0), # LAYER
ev3.LCX(port), # NO
ev3.LCX(30), # TYPE - EV3-IR
ev3.LCX(0), # MODE - Proximity
ev3.LCX(1), # VALUES
ev3.GVX(0) # VALUE1
])
reply = my_ev3.send_direct_cmd(ops, global_mem=4)
lastUltra = time()
curUltra = struct.unpack('<f', reply[5:])[0]
return curUltra
ev3.cmds.clr.CHANGES,
ev3.LCX(0),
ev3.LCX(0)
])
evie.send_direct_cmd(ops_clear)
ops_read = b''.join([
ev3.op.Input.Device,
ev3.cmds.Ready.SI,
ev3.LCX(0),
ev3.LCX(0),
ev3.LCX(16),
ev3.LCX(0),
ev3.LCX(1),
ev3.GVX(0)
])
ops_open = b''.join([
ev3.op.Output.Speed,
ev3.LCX(0),
ev3.LCX(ev3.port.A),
ev3.LCX(-50),
ev3.op.Output.Start,
ev3.LCX(0),
ev3.LCX(ev3.port.A)
])
ops_close = b''.join([
ev3.op.Output.Speed,
ev3.LCX(0),
import struct
my_ev3 = ev3.EV3(protocol=ev3.BLUETOOTH, host='/dev/tty.EV3-SerialPort')
my_ev3.sync_mode = ev3.SYNC
my_ev3.verbosity = 0
ev3_car = ev3.TwoWheelVehicle(ev3_obj=my_ev3)
ev3_car.verbosity = 1
ev3_car.port_left = ev3.port.B
ev3_car.port_right = ev3.port.C
#ev3_car.move(50,0)
ev3_car.stop()
ops = b''.join(
[ev3.op.Com.Get, ev3.cmds.get.BRICKNAME,
ev3.LCX(16),
ev3.GVX(0)])
ops_clear = b''.join(
[ev3.op.Input.Device, ev3.cmds.clr.CHANGES,
ev3.LCX(0),
ev3.LCX(0)])
ops_read = b''.join([
ev3.op.Input.Device, ev3.cmds.Ready.SI,
ev3.LCX(0),
ev3.LCX(0),
ev3.LCX(16),
ev3.LCX(0),
ev3.LCX(1),
ev3.GVX(0)
])
import ev3, struct, time
my_ev3 = ev3.EV3(protocol=ev3.BLUETOOTH, host='00:16:53:43:1b:92')
print("start")
my_ev3.verbosity = 1
ops_read = b''.join([
ev3.opInput_Device,
ev3.READY_RAW,
ev3.LCX(0), # LAYER
ev3.LCX(1), # NO
ev3.LCX(33), # TYPE - IR
ev3.LCX(1), # MODE - Seeker
ev3.LCX(2), # VALUES
ev3.GVX(0), # VALUE1 - heading channel 1
ev3.GVX(4) # VALUE2 - proximity channel 1
])
speed_ctrl = ev3.PID(0, 2, half_life=0.1, gain_der=0.2)
while True:
reply = my_ev3.send_direct_cmd(ops_read, global_mem=8)
(heading, proximity) = struct.unpack('2i', reply[5:])
print("Distance (in CM) =",heading)
# print(proximity)
time.sleep(0.250)
# if proximity == -2147483648:
# print("**** lost connection ****")
# break
import ev3, struct
my_ev3 = ev3.EV3(protocol=ev3.BLUETOOTH, host='00:16:53:43:1b:92')
my_ev3.verbosity = 1
ops = b''.join([
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0), # LAYER
ev3.port_motor_input(ev3.PORT_B), # NO
ev3.LCX(7), # TYPE
ev3.LCX(0), # MODE
ev3.LCX(1), # VALUES
ev3.GVX(0), # VALUE1
ev3.opInput_Device,
ev3.READY_RAW,
ev3.LCX(0), # LAYER
ev3.port_motor_input(ev3.PORT_C), # NO
ev3.LCX(7), # TYPE
ev3.LCX(0), # MODE
ev3.LCX(1), # VALUES
ev3.GVX(4) # VALUE1
])
reply = my_ev3.send_direct_cmd(ops, global_mem=8)
(pos_a, pos_d) = struct.unpack('<fi', reply[5:])
print("positions in degrees (ports B and C): {} and {}".format(pos_a, pos_d))
