command_queue.put((Order.SERVO, int((THETA_MIN + THETA_MAX) / 2))) if __name__ == '__main__': serial_file = None try: # Open serial port (for communication with Arduino) serial_file = open_serial_port(baudrate=BAUDRATE) 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") # Create Command queue for sending orders command_queue = CustomQueue(COMMAND_QUEUE_SIZE) n_received_semaphore = threading.Semaphore(N_MESSAGES_ALLOWED) # Lock for accessing serial file (to avoid reading and writing at the same time) serial_lock = threading.Lock()
import os 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))
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 write_order, Order from robust_serial.threads import CommandThread, ListenerThread from robust_serial.utils import open_serial_port, CustomQueue ''' def reset_command_queue(): """ Clear the command queue LOL just realizized new handshake: python starts by sending, arduino starts by listening... eh actully nah """ command_queue.clear() ''' if __name__ == '__main__': try: serial_file = open_serial_port(serial_port="/dev/cu.usbmodem1411", baudrate=115200, timeout=None) except Exception as e: raise e is_connected = False #print("Order.HEllo evaluates to: ",Order.HELLO) while not is_connected: write_order( serial_file, Order.HELLO ) #basically sends one byte representing the numbe that the broadcast order corresponds to (Hello --> 00000000) bytes_array = bytearray(serial_file.read(1)) if bytes_array[0] == Order.ALREADY_CONNECTED: is_connected = True print("connected!!")
import time from robust_serial import write_order, Order, write_i8, write_i16, read_i8, read_order from robust_serial.utils import open_serial_port if __name__ == '__main__': try: serial_file = open_serial_port(baudrate=115200, 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 # Equivalent to write_i8(serial_file, Order.MOTOR.value) write_order(serial_file, Order.MOTOR) write_i8(serial_file, motor_speed)
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))
def start(ardList): for ard in ardList: ard.conn.reset_input_buffer() ard.conn.reset_output_buffer() print(ard) write_order(ard.conn, Order.ALLSTART)
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()
from __future__ import print_function, division, absolute_import import serial import time import struct from Arduino import Arduino from robust_serial import write_order, Order, write_i8, write_i16, read_i8, read_order ard1 = Arduino(port="/dev/ttyACM0", speed=9600) ser = ard1.conn ser.reset_input_buffer() 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))