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)