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))
