def detect_ultrasonic():
global flag_obstacle, capteur, count_obstacle
#print("flag_obstacle ",flag_obstacle)
capteur = read_i8(serial_file) # recupere la positison du capteur
#print(capteur)
if capteur == 4 and flag_obstacle == 0: # s'il y a un obstaccle
flag_obstacle = 1 # on bloque jusqu'à ce qu'il n'y ait plus d'obstacle
print("obstacle detecte ...")
write_order(serial_file, Order.STOP)
time.sleep(5)
## msg={"from":name,"info":"obstacle"}
## reqsock.send_pyobj(msg)
## rep=reqsock.recv_pyobj()
if capteur == 4 and flag_obstacle == 1 and path != 8:
change_path()
demi_tour()
if (capteur == 100 and flag_obstacle
== 1): # si on est sur qu'il n'y a plus d'obstacle
flag_obstacle = 0 # on debloque jusqu'au nouvel obstacle
def receive_messages():
"""
Receive messages (server side)
"""
server_sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
server_sock.bind(("", CHANNEL))
print("Waiting for client...")
# Wait for client
server_sock.listen(1)
client_sock, client_address = server_sock.accept()
print("Accepted connection from {}".format(client_address))
# Rename function to work with the lib
client_sock.read = client_sock.recv
for i in range(10):
print("Received (i8): {}".format(read_i8(client_sock)))
big_number = read_i32(client_sock)
print("Received (i32): {}".format(big_number))
client_sock.close()
server_sock.close()
def receive_messages():
"""
Receive messages (server side)
"""
server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server_socket.bind(('', PORT))
print("Waiting for client...")
# Wait for client
server_socket.listen(1)
client_sock, client_address = server_socket.accept()
print("Accepted connection from {}".format(client_address))
# Wrap socket to work with the lib
client_sock = SocketAdapter(client_sock)
for i in range(10):
print("Received (i8): {}".format(read_i8(client_sock)))
big_number = read_i32(client_sock)
print("Received (i32): {}".format(big_number))
print("Server exiting...")
client_sock.close()
server_socket.close()
def test_read_write_orders():
motor_speed = -57
servo_angle = 512 # 2^9
big_number = -32768 # -2^15
f = TemporaryFile()
# Equivalent to write_i8(f, Order.MOTOR.value)
write_order(f, Order.MOTOR)
write_i8(f, motor_speed)
write_order(f, Order.SERVO)
write_i16(f, servo_angle)
write_order(f, Order.ERROR)
write_i32(f, big_number)
f.seek(0, 0)
# Equivalent to Order(read_i8(f))
read_1st_order = read_order(f)
read_motor_speed = read_i8(f)
read_2nd_order = read_order(f)
read_servo_angle = read_i16(f)
read_3rd_order = read_order(f)
read_big_number = read_i32(f)
assert_eq(read_1st_order, Order.MOTOR)
assert_eq(read_motor_speed, motor_speed)
assert_eq(read_2nd_order, Order.SERVO)
assert_eq(read_servo_angle, servo_angle)
assert_eq(read_3rd_order, Order.ERROR)
assert_eq(read_big_number, big_number)
def setSpeed(ard, motor, speed):
write_order(ard.conn, Order.MOTOR)
write_i8(ard.conn, motor)
write_i8(ard.conn, speed // 10)
print(read_i8(ard.conn))
def readAll(ardList):
for ard in ardList:
ard.conn.reset_output_buffer()
print(ard)
print(read_i8(ard.conn))
def main():
for arduino_port in arduinoPorts:
try:
ser = open_serial_port(serial_port=arduino_port.device ,baudrate=baudRate, timeout=1)
serial_file.append(ser)
except (OSError, serial.SerialException):
pass
# Wait 3 seconds until Arduino is ready because Arduino resets after serial USB connection
time.sleep(3)
# create a list of all no compatible Arduinos and no Arduinos
serDel = []
IDS = []
# Send a command to verify if it is a compatible Arduino
for ser in serial_file:
write_order(ser, Order.HELLO)
try:
dataIn = read_i8(ser)
if dataIn>0 and dataIn<10:
# It is a compatible Arduino
print("Compatible Arduino ID: "+str(dataIn)+" -> "+str(ser.port))
IDS.append(str(dataIn))
else:
# No return a valid Data so it is not a compatible arduino
ser.close()
serDel.append(ser)
print("No compatible Arduino -> "+str(ser.port))
except (struct.error, OverflowError):
#There is no new data from slave, so it is not an Arduino
ser.close()
serDel.append(ser)
#print("No Arduino -> "+str(ser.port))
pass
# Delete all no compatibe Arduinos and no Arduinos from serial_file
for delete in serDel:
serial_file.remove(delete)
if not len(serial_file)>0:
print ("There are not compatible Arduinos connected !!")
else:
while True:
print("===============================================================")
print("Arduinos Available: ", IDS)
user_input = input("Choose Arduino ID: ")
try:
index_ids=IDS.index(str(user_input))
break
except ValueError:
print("Invalid ID.")
continuos=False
try:
while True:
print("===============================================================")
print(" SPEED TEST")
print("Caution: If you have already calibrated ESC and want another calibration, first desconnect ESC from power source")
print("0: Normal Operation, if you calibrated before")
print("1: Calibration")
print("2: Quit")
print("===============================================================")
user_input = input("Enter mode number and then enter: ")
try:
mode= int(user_input)
if mode == 2:
break
elif mode == 0:
write_order(serial_file[index_ids],Order.START)
createCSV()
time.sleep(5)
continuos=True
break
elif mode == 1:
write_order(serial_file[index_ids],Order.CALIBRATION)
createCSV()
print("Now connect the ESC to power and wait some seconds")
time.sleep(12)
continuos=True
break
else:
print("No valid mode")
continue
except ValueError:
print("Invalid Value.")
pass
except KeyboardInterrupt:
pass
try:
print("Arduino data are been collected at "+inputCSV)
print("Press KeyboardInterrupt Ctl+c for quit")
count=0
line1 = []
while continuos==True:
write_order(serial_file[index_ids], Order.DATA)
speed = read_i16(serial_file[index_ids])
setpoint = read_i16(serial_file[index_ids])
control = read_i16(serial_file[index_ids])
data = {csvHeaderIndex: count,
csvHeaderTime: datetime.datetime.now(),
csvHeaderRPM: speed,
csvHeaderSetpoint: setpoint,
csvHeaderControl: control
}
line1 = appeandNewRow(data, line1)
count=count+1
pass
except KeyboardInterrupt:
pass
write_order(serial_file[index_ids],Order.STOP)
serial_file[index_ids].close()