def move(cx):
erreur = cx - 160
xmax = 160
vmax_droit = 55 #55 marche pour les croix
v_min_turn = 90 #90
v_max_turn = 92 #95
err_norm = erreur / xmax
lim = 0.35
if 0 < err_norm < lim:
v_droite = int(vmax_droit * (1 - err_norm))
v_gauche = vmax_droit
elif -lim <= err_norm < 0:
v_droite = vmax_droit
v_gauche = int(vmax_droit * (1 - err_norm))
elif err_norm > lim:
lmbda = (err_norm - lim) / (1 - lim)
v_droite = -int(v_min_turn * lmbda + v_max_turn * (1 - lmbda))
v_gauche = int(v_min_turn * lmbda + v_max_turn * (1 - lmbda))
else:
lmbda = (abs(err_norm) - lim) / (1 - lim)
v_droite = int(v_min_turn * lmbda + v_max_turn * (1 - lmbda))
v_gauche = -int(v_min_turn * lmbda + v_max_turn * (1 - lmbda))
#print("erreur norm = %0.2f"%(err_norm*100),"%")
#print('v_gauche=%d , v_droite=%d'%(v_gauche,v_droite))
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, v_droite) # vitesse du moteur de droite
write_i8(serial_file, v_gauche) # vitesse du moteur de gauche
def demi_tour():
global direction
direction = (direction + 2) % 4
v = 100
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, v) # vitesse du moteur de droite
write_i8(serial_file, -v) # vitesse du moteur de gauche
time.sleep(3)
write_order(serial_file, Order.STOP)
def run(self):
try:
serial_file = open_serial_port(baudrate=1000000, timeout=None)
except Exception as e:
raise e
is_connected = False
# Initialize communication with Arduino
while not is_connected:
print("Waiting for arduino...")
write_order(serial_file, Order.HELLO)
bytes_array = bytearray(serial_file.read(1))
if not bytes_array:
time.sleep(2)
continue
byte = bytes_array[0]
if byte in [Order.HELLO.value, Order.ALREADY_CONNECTED.value]:
is_connected = True
print("Connected to Arduino")
motor_speed = -56
#start_time = millis();
while True:
start_time = millis()
led_state = 0
if (start_time % 2000) > 1000:
led_state = 4095
# Equivalent to write_i8(serial_file, Order.MOTOR.value)
for i in range(64):
write_order(serial_file, Order.SERVO)
write_i16(serial_file, led_state)
# write_i16(serial_file, random.randint(0, 4095))
#order = read_order(serial_file)
delayMicroseconds(300)
#print("Ordered received: {:?}", order)
#time.sleep(1)
#for _ in range(1):
#order = read_order(serial_file)
#if(order == Order.RECEIVED):
# index += 1
# print("RECEIVED: ", index)
#print("Ordered received: {:?}", order)
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, 100)
#order = read_order(serial_file)
elapsed_time = millis() - start_time
print("TOTAL TIME: ", elapsed_time)
def callback(data):
#Callback function of subscribed topic.
global prev_datal, prev_datar
# Equivalent to write_i8(serial_file, Order.MOTOR.value)
if data.L != prev_datal or data.R != prev_datar:
write_order(serial_file, Order.MOTOR1)
write_i8(serial_file, int(data.L))
write_order(serial_file, Order.MOTOR2)
write_i8(serial_file, int(data.R))
prev_datal = data.L
prev_datar = data.R
def send_messages(mac_address):
"""
Send messages (client side)
:param mac_address: (str)
"""
socket = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
socket.connect((mac_address, CHANNEL))
print("Connected to {}".format(mac_address))
# Rename function to work with the lib
socket.write = socket.send
for i in range(10):
write_i8(socket, i)
write_i32(socket, 32768)
socket.close()
def send_messages(server_address):
"""
Send messages (client side)
:param server_address: (str)
"""
client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
client_socket.connect((server_address, PORT))
# Wrap socket to work with the lib
client_socket = SocketAdapter(client_socket)
print("Connected to {}".format(server_address))
for i in range(10):
write_i8(client_socket, i)
write_i32(client_socket, 32768)
print("Client exiting...")
client_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)
elif turn_instruction == "straight":
straight = True
straight_start_time = time.time()
# straight_timer = straight_timer_threshold
elif turn_instruction == "stop":
running = False
else:
deviation = camera_processor.get_deviation()
if deviation is not None:
motor_command = line_command(deviation)
if motor_command is not None:
vit_gauche, vit_droite = motor_command
write_order(serial_file, Order.MOTOR)
vit_droite = max(-100, min(100, vit_droite))
vit_gauche = max(-100, min(100, vit_gauche))
write_i8(serial_file,
vit_droite) # vitesse du moteur de droite
write_i8(serial_file,
vit_gauche) # vitesse du moteur de gauche
#camera_processor.show()
else:
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, 0) # vitesse du moteur de droite
write_i8(serial_file, 0) # vitesse du moteur de gauche
def motor_crtl(data):
rospy.loginfo(rospy.get_caller_id() + "motor_crtl: I heard %s", data.data)
if data.data == 'f':
# forward
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, 100) #value right motor
write_i8(serial_file, 100) #value left motor
time.sleep(2)
write_order(serial_file, Order.STOP)
elif data.data == 'b':
# backward
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, -100) #value right motor
write_i8(serial_file, -100) #value left motor
time.sleep(2)
write_order(serial_file, Order.STOP)
elif data.data == 'r':
# spin right
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, -100) #value right motor
write_i8(serial_file, 100) #value left motor
time.sleep(2)
write_order(serial_file, Order.STOP)
elif data.data == 'l':
# spin left
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, 100) #value right motor
write_i8(serial_file, -100) #value left motor
time.sleep(2)
write_order(serial_file, Order.STOP)
from robust_serial import Order, write_order, write_i8, write_i16, write_i32, read_i8, read_i16, read_i32, read_order
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Reading / Writing a file')
parser.add_argument('-f',
'--test_file',
help='Test file name',
default="test.txt",
type=str)
args = parser.parse_args()
with open(args.test_file, 'wb') as f:
write_order(f, Order.HELLO)
write_i8(f, Order.MOTOR.value)
write_i16(f, -56)
write_i32(f, 131072)
with open(args.test_file, 'rb') as f:
# Equivalent to Order(read_i8(f))
order = read_order(f)
print(order)
motor_order = read_order(f)
print(motor_order)
print(read_i16(f))
print(read_i32(f))
os.remove(args.test_file)
except Exception as e:
raise e
is_connected = False
# Initialize communication with Arduino
while not is_connected:
print("Waiting for arduino...")
write_order(serial_file, Order.HELLO)
bytes_array = bytearray(serial_file.read(1))
if not bytes_array:
time.sleep(2)
continue
byte = bytes_array[0]
if byte in [Order.HELLO.value, Order.ALREADY_CONNECTED.value]:
is_connected = True
print("Connected to Arduino")
motor_speed = -56
# Equivalent to write_i8(serial_file, Order.MOTOR.value)
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, motor_speed)
write_order(serial_file, Order.SERVO)
write_i16(serial_file, 120)
for _ in range(10):
order = read_order(serial_file)
print("Ordered received: {:?}", order)
is_connected = False
# Initialize communication with Arduino
while not is_connected:
write_order(serial_file, Order.HELLO)
bytes_array = bytearray(serial_file.read(1))
if not bytes_array:
time.sleep(2)
continue
byte = bytes_array[0]
if byte in [Order.HELLO.value, Order.ALREADY_CONNECTED.value]:
is_connected = True
print('start motors')
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, 100) #valeur moteur droit
write_i8(serial_file, 100) #valeur moteur gauche
time.sleep(2)
write_order(serial_file, Order.STOP)
time.sleep(2)
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, -100) #valeur moteur droit
write_i8(serial_file, -100) #valeur moteur gauche
time.sleep(2)
write_order(serial_file, Order.STOP)
camera.stop_preview()
camera.close()
def traite_commande(commande):
global direction, cur_pos
turn_time = 2.3
if commande == "d":
direction = (direction + 1) % 4 #sens horaire
v = 100
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, -v) # vitesse du moteur de droite
write_i8(serial_file, v) # vitesse du moteur de gauche
time.sleep(turn_time)
elif commande == "g":
direction = (direction - 1) % 4
v = 100
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, v) # vitesse du moteur de droite
write_i8(serial_file, -v) # vitesse du moteur de gauche
time.sleep(turn_time)
elif commande == "t":
v = 60
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, v) # vitesse du moteur de droite
write_i8(serial_file, v) # vitesse du moteur de gauche
elif commande == "turn":
demi_tour()
elif commande == 's':
demi_tour()
write_order(serial_file, Order.STOP)
update_position()
def process_msg(msg):
global cur_pos
global liste_commandes, path
###############################################################
# verifie si le message est destiné au robot
# s'il y a un champ 'to' et que le nom n'est pas celui du robot
# on oublie le message
###############################################################
if "to" in msg and msg["to"] != name:
print("this msg wasn't for me...")
##############################################################
# sinon traitement des différents messages reçus
###############################################################
else:
if "command" in msg:
#########################################
# quand on reçoit un start
#########################################
vmax = 60
if msg["command"] == "start":
print("the server orders me to start")
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, vmax) # vitesse du moteur de droite
write_i8(serial_file, vmax) # vitesse du moteur de gauche
if msg["command"] == "backwards":
print("the server orders me to go backwards")
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, -100)
write_i8(serial_file, -100)
if msg["command"] == "right":
print("the server orders me to go right")
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, -100)
write_i8(serial_file, 100)
if msg["command"] == "left":
print("the server orders me to go right")
write_order(serial_file, Order.MOTOR)
write_i8(serial_file, 100)
write_i8(serial_file, -100)
if msg["command"] == "goto":
print("the server orders me to go to destination")
liste_commandes = msg["liste_commandes"]
#liste_commandes.append('s')
liste_noeuds = msg["liste_noeuds"]
print(liste_commandes)
if msg["command"] == "takecommands":
print("the server orders me to take his commands")
liste_commandes = list(msg["liste_commandes"])
print(liste_commandes)
if msg["command"] == "always_forward":
print("the server orders me to go forward always")
liste_commandes = ["t"] * 100
path = 8
if msg["command"] == "servo":
print("the server orders SERVO")
write_order(serial_file, Order.SERVO)
write_i8(serial_file, 60)
#########################################
# quand on reçoit un stop
#########################################
if msg["command"] == "stop":
print("the server orders me to stop")
liste_commandes = []
write_order(serial_file, Order.STOP)
cur_pos = [0, -1]
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))
is_connected = False
while not is_connected:
print("Waiting for arduino...")
write_order(ser, Order.HELLO)
bytes_array = bytearray(ser.read(1))
if not bytes_array:
time.sleep(2)
continue
byte = bytes_array[0]
if byte in [Order.HELLO.value, Order.ALREADY_CONNECTED.value]:
is_connected = True
print("Connected!")
write_order(ser, Order.HELLO)
write_order(ser, Order.MOTOR)
write_i8(ser, 1)
write_i8(ser, 90)
while True:
bytes_array = bytearray(ser.read(1))
if not bytes_array:
time.sleep(2)
print("no messages")
continue
byte = bytes_array[0]
# byte = ser.read(1)
print(byte)
time.sleep(1)
# write_order(ser, Order.MOTOR)
# write_i8(ser, 1)
# write_i8(ser, 90)
