def main():
a = 0
matriceCouleurs = [[]]
tabCouleurs = []
liste = []
liste2 = []
# création instance
filin = open("fichierCouleurs.txt", "a")
i2c = busio.I2C(board.SCL, board.SDA)
tca = adafruit_tca9548a.TCA9548A(i2c, 0x70)
# erreur viens de l'adresse donner en parametres
#tca2 = adafruit_tca9548a.TCA9548A(i2c, 0x71)
#création des capteurs en tableau avec affectation valeurs utilisable
for x in range(0, 4):
liste.append(adafruit_tcs34725.TCS34725(tca[x]))
liste[x].gain = 16
liste[x].integration_time = 200
print(
"Chaque capteur possède un gain = 16 et un temps d'intégration = 200ms"
)
# récupération data et test de la couleurs qui lui est lié
for x in range(0, 4):
data = liste[x].color_raw
dataLux = liste[x].lux
with open("fichierCouleurs.txt", "a") as filout:
filout.write("\n" + str(data))
print("data0 = ", data)
print("Luminosité0 = ", dataLux)
tabCouleurs.append(testCouleurs(data))
# changement multiplexeur
tca = adafruit_tca9548a.TCA9548A(i2c, 0x71)
for x in range(0, 4):
liste2.append(adafruit_tcs34725.TCS34725(tca[x]))
liste2[x].gain = 16
liste2[x].integration_time = 200
for x in range(0, 4):
data2 = liste2[x].color_raw
dataLux2 = liste2[x].lux
with open("fichierCouleurs.txt", "a") as filout:
filout.write("\n" + str(data2))
print("data1 = ", data2)
print("Luminosité1 = ", dataLux2)
tabCouleurs.append(testCouleurs(data2))
print(tabCouleurs)
for x in range(4):
for i in range(2):
matriceCouleurs[x].append(tabCouleurs[i])
def lecture_capteurs(listeCapteurs, tabCouleurs, i2c):
for x in range(0, 6):
if (x < 2):
tca = adafruit_tca9548a.TCA9548A(i2c)
tabCouleurs.append(testCouleurs(listeCapteurs[x].color_raw))
print(listeCapteurs[x].color_raw)
else:
tca = adafruit_tca9548a.TCA9548A(i2c, 0x71)
tabCouleurs.append(testCouleurs(listeCapteurs[x].color_raw))
print(listeCapteurs[x].color_raw)
return tabCouleurs
def verif(liste, liste2, pinBTN1, pinBTN2,
pinLED): #pour recuperer le résultat 2e tour
calculPose = ""
tabCouleurs = []
i2c = busio.I2C(board.SCL, board.SDA)
#attente
while (True):
tca = adafruit_tca9548a.TCA9548A(i2c)
GPIO.setup(pinBTN1, GPIO.IN, pull_up_down=GPIO.PUD_UP
) # bouton poussoir 1 Capter + Envoie donnée
GPIO.setup(pinLED, GPIO.OUT)
# création des capteurs en tableau avec affectation valeurs utilisable
dataL = liste[0].lux
print(dataL)
if (dataL < 5):
GPIO.output(pinLED, not GPIO.input(pinLED)
) # changement d'état (allumer normalement)
etat1 = GPIO.input(pinBTN1) # récupération état du bouton envoie data
etat2 = GPIO.input(pinBTN2) # récupération état du bouton exo
if (etat1 == 0 and etat2
== 1): # permet de ne pas activer les 2 boutons en même temps
# fonctionnement du programme de récupération des couleurs (charactère)
# récupération data et test de la couleurs qui lui est lié
for x in range(0, 2):
time.sleep(0.5) # laisser le temps de se mettre à la lumière
data = liste[
x].color_raw # récup valeurs capteur multiplexeur 1
print(data)
tabCouleurs.append(testCouleurs(data))
# changement multiplexeur
tca = adafruit_tca9548a.TCA9548A(i2c, 0x71)
for x in range(0, 4):
data1 = liste2[
x].color_raw # récup valeurs capteur multiplexeur 2
print("data1 = ", data1)
tabCouleurs.append(testCouleurs(data1))
# remplissage matrice couleurs
matriceCouleurs = [tabCouleurs[i:i + 2] for i in range(0, 6, 2)]
# on fait la range avec le pas que l'on souhaite
print(matriceCouleurs)
calculPose += testCaractere(matriceCouleurs)
return (calculPose)
else:
print("attente resultat")
time.sleep(0.5)
def definition_capteurs(i2c, listeCapteurs):
# premiere fournée de capteurs
for x in range(0, 6):
if (x < 2): #0 et 1 affecter au tca 2
tca = adafruit_tca9548a.TCA9548A(i2c, 0x70)
listeCapteurs.append(adafruit_tcs34725.TCS34725(tca[x]))
listeCapteurs[x].gain = 16
listeCapteurs[x].integration_time = 200
else: # les autres sont affecter avec le deuxième multiplexeur
tca = adafruit_tca9548a.TCA9548A(i2c, 0x71)
listeCapteurs.append(adafruit_tcs34725.TCS34725(tca[x - 2]))
listeCapteurs[x].gain = 16
listeCapteurs[x].integration_time = 200
return listeCapteurs
def __init__(self,
group=None,
target=None,
name=None,
args=(),
kwargs=None):
super(I2CThread, self).__init__(group=group, target=target, name=name)
self._running = True
self.args = args
self.kwargs = kwargs
self.queue = kwargs['queue']
print("I2CThread: initializing I2C")
self.i2c = busio.I2C(board.SCL, board.SDA)
print('I2CThread: Initializing I2C mux')
self.tca = adafruit_tca9548a.TCA9548A(self.i2c)
self.drv = []
for i in range(8):
print('I2CThread: Initializing driver {}'.format(i))
self.drv.append(adafruit_drv2605.DRV2605(self.tca[i]))
self.drv[i].use_LRM()
self.drv[i].library = adafruit_drv2605.LIBRARY_LRA
return
def main():
liste = [4]
# création instance
filin = open("fichierCouleurs.txt", "w")
i2c = busio.I2C(board.SCL, board.SDA)
tca = adafruit_tca9548a.TCA9548A(i2c)
sensor0 = adafruit_tcs34725.TCS34725(tca[0])
data0 = sensor0.color_raw
print(data0)
# with open("fichierCouleurs.txt", "w") as filout:
# filout.write(data0)
sensor1 = adafruit_tcs34725.TCS34725(tca[1])
data1 = sensor1.color_raw
print(data1)
# with open("fichierCouleurs.txt", "w") as filout:
# filout.write(data1)
sensor2 = adafruit_tcs34725.TCS34725(tca[2])
data2 = sensor2.color_raw
print(data2)
# with open("fichierCouleurs.txt", "w") as filout:
# filout.write(data2)
sensor3 = adafruit_tcs34725.TCS34725(tca[3])
data3 = sensor3.color_raw
print(data3)
def get_lux_readings(logger):
tsl_dict = {}
lux_dict = {}
try:
i2c = busio.I2C(board.SCL, board.SDA)
tca = adafruit_tca9548a.TCA9548A(i2c)
except Exception as e:
logger.error("error at initiating i2c devices :{}".format(str(e)))
return lux_dict
for i in range(8):
try:
tsl_dict["tsl_{}".format(i)] = adafruit_bh1750.BH1750(tca[i])
except Exception as e:
logger.error(
"error in initiating lux sensor at {}th pin :{}".format(
i, str(e)))
pass
for k, tsl in tsl_dict.items():
try:
lux_dict[k] = tsl.lux
except Exception as e:
logger.error("error in reading lux at {}th sensor :{}".format(
i, str(e)))
lux_dict[k] = -1
return lux_dict
def __init__(self,
group=None,
target=None,
name=None,
args=(),
kwargs=None):
super(I2CThread, self).__init__(group=group, target=target, name=name)
self._running = True
self.args = args
self.kwargs = kwargs
self.queue = kwargs['queue']
print("I2CThread: initializing I2C") # communicating to DRV2605
self.i2c = busio.I2C(board.SCL, board.SDA) # access to hardware
print('I2CThread: Initializing I2C mux'
) # enabling multiple motor control
self.tca = adafruit_tca9548a.TCA9548A(self.i2c)
self.drv = [] # drive multiple motors and link Queue protocol to DRV
for i in range(8):
print('I2CThread: Initializing driver {}'.format(i))
self.drv.append(adafruit_drv2605.DRV2605(self.tca[i]))
#self.drv[i].use_LRA() #adafruit-DRV
#print('DRV2605: Auto-Calibrating driver {}'.format(i))
#self.drv[i].mode = 7 #MODE_AUTOCAL
#self.drv[i].autocal()
#print('2x auto-cal driver {}'.format(i))
#self.drv[i].autocal()
print('DRV2605: driver {} ready for use'.format(i))
self.drv[i].mode = 1 #MODE_INTTRIG
self.drv[i].library = 6 #LIBRARY_LRA
return
def __init__(self, bus):
self.bus = bus
self.mux = adafruit_tca9548a.TCA9548A(bus)
self.imu = lsm6dsox.LSM6DSOX(self.mux[2])
self.rgb_left = rgb_lib.RgbSensor(self.mux[7])
self.rgb_center = rgb_lib.RgbSensor(self.mux[6])
self.rgb_right = rgb_lib.RgbSensor(self.mux[5])
self.motor = motor_lib.MotorController(self.mux[0])
self.laser = laser_lib.LaserController(self.mux[3])
self.pixy = pixy_lib.Pixy(self.mux[4], team='red')
def __init__(self):
self.node = rospy.init_node("light_sensors", anonymous=True)
self.sensor_publisher = rospy.Publisher("sensor_readings",
Float32MultiArray,
queue_size=10)
self.sensor_readings = Float32MultiArray()
# Create I2C bus as normal
self.i2c = busio.I2C(board.SCL, board.SDA)
# Create the TCA9548A object and give it the I2C bus
self.tca = adafruit_tca9548a.TCA9548A(self.i2c)
def main(argv):
i2c = busio.I2C(board.SCL, board.SDA)
# Create the TCA9548A object and give it the I2C bus
tca = adafruit_tca9548a.TCA9548A(i2c)
rwy_font = ImageFont.truetype('/home/pi/in-the-pattern/oled/16x8pxl-mono.ttf', 72)
# multiplexer index for runway number OLEDs
runway = {}
runway['appch'] = 0
runway['dep'] = 1
input_appch = ''
input_dep = ''
clear_oled = "false"
try:
opts, args = getopt.getopt(argv,"ha:d:c:",["approach=","departure=","clear="])
except getopt.GetoptError:
print('rwy.py -a <approach runway number 1-2 digit> -d <departure runway number 1-2 digit>')
sys.exit(2)
for opt, arg in opts:
if opt == '-h':
print('rwy.py -a <approach runway number 1-2 digit> -d <departure runway number 1-2 digit>')
sys.exit()
elif opt in ("-a", "--approach"):
input_appch = arg
elif opt in ("-d", "--departure"):
input_dep = arg
elif opt in ("-c", "--clear"):
clear_oled = arg
# Runway Names
rwy_number = {}
rwy_number['appch'] = input_appch
rwy_number['dep'] = input_dep
for rwy in runway.keys():
rwy_name = adafruit_ssd1306.SSD1306_I2C(128, 64, tca[runway[rwy]])
W, H = (rwy_name.width, rwy_name.height)
rwy_name.fill(0)
rwy_name.show()
rwy_img = Image.new("1", (W, H))
draw_rwy = ImageDraw.Draw(rwy_img)
w, h = draw_rwy.textsize(rwy_number[rwy], font=rwy_font) #Figure out the height and width of runway number so we can center it
draw_rwy.text(((W-w)/2,(H-h)/2), rwy_number[rwy], font=rwy_font, fill=255)
rwy_name.image(rwy_img)
rwy_name.show()
if clear_oled == "true" :
rwy_name.fill(0)
rwy_name.show()
def __init__(self):
""" Set up hardware connection
Sets up connections to the following bus devices over the i2c bus:
- Sets up a TCA9548a to handle the high traffic load on the bus
- Sets up a 8x8 reed_matrix mapped trough 4 MCP23017
- Sets up a 9x9 led_matrix with the interface LedWrapper which uses a HT16k33 and 1 MCP23017
"""
i2c = busio.I2C(board.SCL, board.SDA)
tca = adafruit_tca9548a.TCA9548A(i2c, address=0x71)
self._perform_safe = SafeDecorator.perform_safe_factory(
lambda: setattr(tca.i2c, '_locked', False))
self._lcd = self._perform_safe(lambda: character_lcd.Character_LCD_I2C(
tca[6], 16, 2, address=0x27))
self._perform_safe(setattr)(self._lcd, "backlight", True)
mcp = [
self._perform_safe(lambda i: MCP23017(tca[i], address=0x20))(i)
for i in range(4)
]
# Initialize reed matrix
for file in range(8):
for rank in range(8):
pin_id = file if rank % 2 == 0 else 15 - file
self._board_reed[file][rank] = self._perform_safe(
mcp[rank // 2].get_pin)(pin_id)
self._perform_safe(setattr)(self._board_reed[file][rank],
"direction",
digitalio.Direction.INPUT)
self._perform_safe(setattr)(self._board_reed[file][rank],
"pull", digitalio.Pull.UP)
led_input_mcp = MCP23017(tca[5], address=0x20)
# Map output buttons
self._buttons = []
for pinId in range(8, 13):
self._buttons.append(
self._perform_safe(led_input_mcp.get_pin)(pinId))
self._perform_safe(setattr)(self._buttons[-1], "direction",
digitalio.Direction.INPUT)
self._perform_safe(setattr)(self._buttons[-1], "pull",
digitalio.Pull.UP)
# Initialize LED matrix
self._led_wrapper = LedWrapper(matrix.MatrixBackpack16x8(tca[4]),
led_input_mcp, self._perform_safe)
self._led_wrapper.clear()
def L90():
with busio.I2C(board.SCL, board.SDA) as bus:
mux = adafruit_tca9548a.TCA9548A(bus)
imu = lsm6dsox.LSM6DSOX(mux[2])
motor = I2CDevice(mux[0], motor_lib.MOTOR_CTRL_ADDR, probe=False)
read_buff = bytearray(16)
with motor:
motor.write_then_readinto(motor_lib.ROT_L_CMD, read_buff)
angle_z = 0
t1 = time.time()
while abs(angle_z) < abs(math.pi / 2) - 0.05:
t2 = time.time()
d_theta = imu.gyro[2]
angle_z += d_theta * (t2 - t1)
t1 = t2
with motor:
motor.write_then_readinto(motor_lib.STOP_CMD, read_buff)
def read_all_sensors(): # Create I2C bus as normal i2c = busio.I2C(board.SCL, board.SDA) # Create the TCA9548A object and give it the I2C bus tca = adafruit_tca9548a.TCA9548A(i2c) # For each sensor, create it using the TCA9548A channel instead of the I2C object tsl0 = adafruit_tsl2591.TSL2591(tca[0]) tsl1 = adafruit_tsl2591.TSL2591(tca[1]) tsl2 = adafruit_tsl2591.TSL2591(tca[2]) tsl3 = adafruit_tsl2591.TSL2591(tca[3]) tsl4 = adafruit_tsl2591.TSL2591(tca[4]) tsl5 = adafruit_tsl2591.TSL2591(tca[5]) tsl6 = adafruit_tsl2591.TSL2591(tca[6]) tsl7 = adafruit_tsl2591.TSL2591(tca[7]) sensor_reading = [tsl0.lux, tsl1.lux, tsl2.lux, tsl3.lux, tsl4.lux, tsl5.lux, tsl6.lux, tsl7.lux] return sensor_reading
def Travel(distance):
distance = distance / 100
with busio.I2C(board.SCL, board.SDA) as bus:
mux = adafruit_tca9548a.TCA9548A(bus)
imu = lsm6dsox.LSM6DSOX(mux[2])
motor = I2CDevice(mux[0], motor_lib.MOTOR_CTRL_ADDR, probe=False)
read_buff = bytearray(16)
with motor:
motor.write_then_readinto(motor_lib.FORWARD_CMD, read_buff)
displacement = 0
velocity = 0
t1 = time.time()
while abs(displacement) < (abs(distance) + math.sqrt(distance * 2.5)):
t2 = time.time()
d_vel = math.sqrt(imu.acceleration[0]**2 * imu.acceleration[1]**2)
velocity += d_vel * (t2 - t1)
displacement += velocity * (t2 - t1)
t1 = t2
with motor:
motor.write_then_readinto(motor_lib.STOP_CMD, read_buff)
def run_healthcheck(cargo="undefined"):
"""[Healthcheck starten]
Arguments:
cargo {String} -- Cargo Parameter (default: {undefined})
Returns:
Boolean -- True, falls Healtcheck OK.
False, falls Fehler beim Healthcheck (siehe Log).
"""
print(
"Class Healthcheck not productive yet (99 percent done, testing ausstehend)."
)
##define flags for nice headers
cpu_flag, ram_flag, sonic_flag, tof1_flag, tof2_flag, camera_flag, mc_flag, passed_flag = "NA", "NA", "NA", "NA", "NA", "NA", "NA", "NA"
findings = 0
#Getting CPU
CPU_usage = getCPUuse()
#Getting RAM
RAM_stats = getRAMinfo()
RAM_free = round(int(RAM_stats[2]) / 1000, 1)
#Sollte kleiner als 10% sein
if (float(CPU_usage) >= 10):
logger.warn("CPU Usage too high!")
findings += 1
cpu_flag = CPU_usage
logger.info("CPU Usage: %s", str(CPU_usage))
#Sollte grösser als 300 sein
if (RAM_free <= 300):
logger.warn("Not enough RAM available!")
findings += 1
ram_flag = RAM_free
logger.info("RAM Free: %s", str(RAM_free))
#Get python prozesses
python_procs = "NA"
try:
python_procs = get_pid("python")
logger.warn("The follow Python-Processes are already running: ",
python_procs)
findings += 1
except:
logger.info("No python running, good2go!")
#Check if Camera Available
camera_result = (subprocess.check_output(
"vcgencmd get_camera",
shell=True)).decode("utf-8").rstrip() #decode and remove \n
if ((camera_result.rsplit(" ", 2)[0].split("=", 1)[1]) == "1"
and (camera_result.rsplit(" ", 2)[1].split("=", 1)[1]) == "1"):
camera_flag = True
logger.info("Camera ready! 📷")
else:
camera_flag = False
logger.warn("Camera not available! �")
findings += 1
#Check sonic
p = multiprocessing.Process(
target=check_sonic,
name="checksonic") #remember, pass functions without ()
p.start()
p.join(3)
if p.is_alive():
logger.warn("Sonic Check takes too long")
p.terminate()
p.join()
sonic_flag = False
findings += 1
else:
sonic_flag = check_sonic()
logger.info("Sonic test passed")
#Check TOF Multiplexer
try:
i2c = busio.I2C(board.SCL, board.SDA)
# Create the TCA9548A object and give it the I2C bus
tca = adafruit_tca9548a.TCA9548A(i2c)
logger.warn("I2C Multiplexer Device (0) Available (probably)")
tca_flag = True
except:
logger.warn("I2C Multiplexer Device (0) Unavailable")
findings += 1
tca_flag = False
#Check TOF 1
try:
i2c = busio.I2C(board.SCL, board.SDA)
tca = adafruit_tca9548a.TCA9548A(i2c)
sensor_tof1 = adafruit_vl6180x.VL6180X(tca[0])
logger.info("I2C Device (1) Available")
tof1_flag = True
except:
logger.warn("I2C Device (1) Unavailable")
findings += 1
tof1_flag = False
#Check TOF 2
try:
i2c = busio.I2C(board.SCL, board.SDA)
tca = adafruit_tca9548a.TCA9548A(i2c)
sensor_tof2 = adafruit_vl6180x.VL6180X(tca[1])
logger.info("I2C Device (2) Available")
tof2_flag = True
except:
logger.warn("I2C Device (1) Unavailable")
findings += 1
tof2_flag = False
#Check MC-COM
#mc_flag = check_microcontroller()
print("MC healtcheck bypassed!!")
mc_flag = True
if (mc_flag):
logger.info("MC is online and well 🤖!")
else:
logger.warn("MC is offline!")
findings += 1
#Validating Findings
if (findings > 0):
passed_flag = False
else:
passed_flag = True
logger.info("Healthcheck completed with '%s' findings", str(findings))
#Print Header Message
nice_headers.print_nice_headers(passed_flag, cpu_flag, ram_flag, tof1_flag,
tof2_flag, sonic_flag, camera_flag,
mc_flag)
return passed_flag
import board
import digitalio
from PIL import Image, ImageDraw, ImageFont
import adafruit_ssd1306
import adafruit_tca9548a
from time import sleep
# Display size
WIDTH = 128
HEIGHT = 64
BORDER = 5
i2c = board.I2C()
tca = [
adafruit_tca9548a.TCA9548A(i2c, address=0x70),
adafruit_tca9548a.TCA9548A(i2c, address=0x71)
]
disp = [] # Display list
#Set up displays
for i in range(9):
try:
disp.append(
adafruit_ssd1306.SSD1306_I2C(WIDTH,
HEIGHT,
tca[i // 7][i % 7],
addr=0x3c))
except Exception as e:
disp.append(None)
print('oled setup failed, index ', i, 'on tca module 0-2: ', i // 7)
import time
import typer
import adafruit_tca9548a
import adafruit_veml6070
import board
import busio
# Initialize I2C bus and sensor.
i2c = busio.I2C(board.SCL, board.SDA)
# mux
tca2 = adafruit_tca9548a.TCA9548A(i2c, address=0x72)
# each sensor
uv_sensor = adafruit_veml6070.VEML6070(tca2[4])
def main(num: int = 100):
for i in range(num):
try:
uv = uv_sensor.uv_raw
except OSError:
uv = None
print(f"uv. {uv: 0.3f}")
time.sleep(1.0)
if __name__ == "__main__":
def main():
#test bdd
nom = "Pablo"
prenom = "Emilio"
calculDonne = "12/6"
calculPosee = "12/6"
resultat = ""
resultatDonne = "2"
resultatEnvoie = ""
tour = 0
juste = 0
calculPose = ""
tabCouleurs = []
liste = []
liste2 = []
dataL = 0
attente = 0
# création instances
pinBTN1 = 21 #recup data led
pinBTN2 = 23 #demande exo
pinBTN3 = 17 #oui (true)
pinBTN4 = 27 #non (false)
pinLED = 16 #manip led
etat3 = 0
etat4 = 0
GPIO.setmode(GPIO.BCM)
GPIO.setup(
pinBTN1, GPIO.IN,
pull_up_down=GPIO.PUD_UP) #bouton poussoir 1 Capter + Envoie donnée
GPIO.setup(pinBTN2, GPIO.IN,
pull_up_down=GPIO.PUD_UP) #bouton poussoir 2 Exo
GPIO.setup(pinLED, GPIO.OUT) #manip led
GPIO.setup(pinBTN3, GPIO.IN, pull_up_down=GPIO.PUD_UP) #Bouton True
GPIO.setup(pinBTN4, GPIO.IN, pull_up_down=GPIO.PUD_UP) #bouton False
while True: #programme continue
while True: # attente
GPIO.output(pinLED, False) #led eteinte
i2c = busio.I2C(board.SCL, board.SDA)
tca = adafruit_tca9548a.TCA9548A(i2c)
#création des capteurs en tableau avec affectation valeurs utilisable
if (tour == 0): #si c'est le premier tour on déclarre
for x in range(0, 2):
liste.append(adafruit_tcs34725.TCS34725(tca[x]))
liste[x].gain = 16
liste[x].integration_time = 200
tour += 1 # tour = nb capteurs penser à changer
dataL = liste[0].lux
if (dataL < 5):
GPIO.output(pinLED, not GPIO.input(pinLED)
) # changement d'état (allumer normalement)
etat1 = GPIO.input(
pinBTN1) #récupération état du bouton envoie data
etat2 = GPIO.input(pinBTN2) #récupération état du bouton exo
etat3 = GPIO.input(pinBTN3)
etat4 = GPIO.input(pinBTN4)
if (etat1 == 0 and etat2 == 1
): #permet de ne pas activer les 2 boutons en même temps
# fonctionnement du programme de récupération des couleurs (charactère)
# récupération data et test de la couleurs qui lui est lié
for x in range(0, 2):
time.sleep(
0.5) #laisser le temps de se mettre à la lumière
data = liste[
x].color_raw #récup valeurs capteur multiplexeur 1
print(data)
tabCouleurs.append(testCouleurs(data))
# changement multiplexeur
tca = adafruit_tca9548a.TCA9548A(i2c, 0x71)
if (tour == 1
): #deuxième remplissage (tour = nb capteur avant !)
for x in range(0, 4):
liste2.append(adafruit_tcs34725.TCS34725(tca[x]))
liste2[x].gain = 16
liste2[x].integration_time = 200
tour += 1
for x in range(0, 4):
data1 = liste2[
x].color_raw #récup valeurs capteur multiplexeur 2
print("data1 = ", data1)
tabCouleurs.append(testCouleurs(data1))
# remplissage matrice couleurs
matriceCouleurs = [
tabCouleurs[i:i + 2] for i in range(0, 6, 2)
]
# on fait la range avec le pas que l'on souhaite
print(matriceCouleurs)
calculPose += testCaractere(matriceCouleurs)
#calcul
print(tabCouleurs)
resultat = eval(calculPose)
if (resultat == eval(calculDonne) and juste == 0):
juste += 1 #si un calcul est juste on peut pas revenir ici
print("Pose le resultat maintenant !")
resultatEnvoie = verif(liste, liste2, pinBTN1, pinBTN2,
pinLED) #resultat pose par l'éléeve
if (resultatEnvoie == resultatDonne
): #resultat donne par l'eleve est juste
print("Bien jouer ! Tu as juste !")
print(eval(resultatEnvoie))
print(eval(calculPosee))
# envoie du calcul à la fin
insert(prenom, nom, calculDonne, calculPose,
resultatEnvoie)
break
else:
print("Faux, veux tu reposer le resultat ?")
insert(prenom, nom, calculDonne, calculPose,
resultatEnvoie)
print(resultatEnvoie)
print(calculPosee)
while (True):
etat3 = GPIO.input(pinBTN3)
etat4 = GPIO.input(pinBTN4)
if (etat3 == 0 or etat4 == 0):
break
if (etat3 == 0
and etat4 == 1): # si appuyer sur btn3 = oui
print("Tu recommence")
print("retry")
break # on remonte juste avec l'exercice en cours
if (etat4 == 0
and etat3 == 1): # si appuyer sur BTN4 = non
print("Tu abandonnes")
print("echec !")
break # fin exercice et en redemander un (peut etre demander avant de remonter)
else:
print("calcul mal poser")
print(
"Veux tu recommencer l'exercice ?") #bouton Oui et Non
etat3 = input(GPIO.input(pinBTN3))
etat4 = input(GPIO.input(pinBTN4))
if (etat3 == 0 and etat4 == 1): #si appuyer sur btn3 = oui
print("retry")
break #on remonte juste avec l'exercice en cours
if (etat4 == 0 and etat3 == 1): #si appuyer sur BTN4 = non
print("echec !")
break #fin exercice et en redemander un (peut etre demander avant de remonter)
break
elif (etat2 == 0 and etat1 == 1):
print("demande exo")
break
elif (etat3 == 0 and etat4 == 1 and etat1 == 1 and etat2 == 1):
exit()
break
else:
print("attente")
time.sleep(0.5)
tabCouleurs = [] #remise à zero du tableau
juste = 0
print("end")
)
# initialize lcd
lcd_rs = digitalio.DigitalInOut(board.D26)
lcd_en = digitalio.DigitalInOut(board.D19)
lcd_d7 = digitalio.DigitalInOut(board.D27)
lcd_d6 = digitalio.DigitalInOut(board.D22)
lcd_d5 = digitalio.DigitalInOut(board.D24)
lcd_d4 = digitalio.DigitalInOut(board.D25)
lcd_columns = 20
lcd_rows = 4
lcd = characterlcd.Character_LCD_Mono(lcd_rs, lcd_en, lcd_d4, lcd_d5, lcd_d6,
lcd_d7, lcd_columns, lcd_rows)
# initialize multiplexer board
tca_multi = TCA.TCA9548A(i2c)
# initialize saffron_grow_sensors
# tca_multi[1] = ADS1115, bme680, ccs811
ads1115 = ADS.ADS1115(tca_multi[1])
bme680_1 = BME.Adafruit_BME680_I2C(tca_multi[1])
ccs811 = CCS.CCS811(tca_multi[1])
# chan0 = AnalogIn(ads1115, ADS.P0)
chan1 = AnalogIn(ads1115, ADS.P1)
chan2 = AnalogIn(ads1115, ADS.P2)
while True:
try:
lcd.clear()
temp_c_room = round(bme680_1.temperature, 1)
temp_f_room = round(((9 / 5) * temp_c_room + 32), 1)
import time
import board
import busio
i2c = busio.I2C(board.SCL, board.SDA)
import adafruit_ads1x15.ads1115 as ADS
import adafruit_tca9548a as TCA
tca = TCA.TCA9548A(i2c)
from adafruit_ads1x15.analog_in import AnalogIn
ads = ADS.ADS1115(tca[2])
try:
while 1:
print("LIDAR1\n")
# print("Nvidia TX2")
chan = AnalogIn(ads,ADS.P0)
volt = chan.voltage
convert = volt*8.3856 ##Conversion for 28V
#convert = volt*4.508
# print("before conversion:", volt, " ")
print("Voltage:", " ", convert, "V, ")
chan = AnalogIn(ads,ADS.P1)
volt1 = chan.voltage
convert1 = volt1/1.5 ##Conversion for 28V
#convert1 = volt1/0.728
# print("before conversion:", volt1," ")
r, g, b, c = self.sensor.color_raw
return (50 >= c > 16 and 24 >= r > 7)
@property
def yellow(self):
r, g, b, c = self.sensor.color_raw
return (c > 50 and r > 24)
if __name__ == "__main__":
try:
read_buff = bytearray(16)
loop_count = 0
with busio.I2C(board.SCL, board.SDA) as bus:
mux = adafruit_tca9548a.TCA9548A(bus)
rgb_left = adafruit_tcs34725.TCS34725(mux[7])
rgb_center = adafruit_tcs34725.TCS34725(mux[6])
rgb_right = adafruit_tcs34725.TCS34725(mux[5])
while 1:
#read rgbs
rl, gl, bl, cl = rgb_left.color_raw
rc, gc, bc, cc = rgb_center.color_raw
rr, gr, br, cr = rgb_right.color_raw
if loop_count % 4 == 0:
print(f'Left: R:{rl} G:{gl} B:{bl} C:{cl}')
print(f'Center: R:{rc} G:{gc} B:{bc} C:{cc}')
print(f'Right: R:{rr} G:{gr} B:{br} C:{cr}')
time.sleep(0.24)
loop_count += 1
def __init__(self,
main_path,
mouthpiece='hamilton',
dp_thresh=0.0,
verbose=False):
# run in verbose mode?
self.verbose = verbose
self.dp_thresh = dp_thresh
# Initialize the i2c bus
self.i2c = busio.I2C(board.SCL, board.SDA)
# Using the adafruit_lps35hw class to read in the pressure sensor
# note the address must be in decimal.
# allowed addresses are:
# 92 (0x5c - if you put jumper from SDO to Gnd)
# 93 (0x5d - default)
# set up the i2c multiplexer
self.i2cmux = adafruit_tca9548a.TCA9548A(self.i2c)
# Set up the sensors
self.sensor2 = adafruit_lps35hw.LPS35HW(self.i2c, address=92)
self.sensor1 = adafruit_lps35hw.LPS35HW(self.i2c, address=93)
self.sensor3 = adafruit_lps35hw.LPS35HW(
self.i2cmux[0]) # this sensor is plugged into the mux on ch 0
self.sensor1.data_rate = adafruit_lps35hw.DataRate.RATE_75_HZ
self.sensor2.data_rate = adafruit_lps35hw.DataRate.RATE_75_HZ
self.sensor3.data_rate = adafruit_lps35hw.DataRate.RATE_75_HZ
self.sensor1.low_pass_enabled = True
self.sensor2.low_pass_enabled = True
self.sensor3.low_pass_enabled = True
# Load the flow calibration polynomial coefficients
self.main_path = main_path
# define the calibration file based on the mouthpiece
self.set_mouthpiece(mouthpiece)
# Define the unit conversion factor
self.mbar2cmh20 = 1.01972
# Load the flow calibration polynomial coefficients
self.main_path = main_path
# initial offsets are zero
self.p1_offset = 0.0
self.p2_offset = 0.0
self.p_ambient = 0.0
# holds info about whether the sensor is initialized
self.initialized = False
# no flow offset initially
self.dp_offset = 0.0
# Zero the sensors
self.rezero()
# Initialize the class values
self.read()
def main(argv):
i2c = busio.I2C(board.SCL, board.SDA)
# Create the TCA9548A object and give it the I2C bus
tca = adafruit_tca9548a.TCA9548A(i2c)
tail_font = ImageFont.truetype(
'/home/pi/in-the-pattern/oled/16x8pxl-mono.ttf', 38)
input_leg = ''
input_tail = ''
clear_oled = "false"
pattern_direction = "l"
try:
opts, args = getopt.getopt(argv, "hl:t:c:p:",
["leg=", "tail=", "clear=", "pattern="])
except getopt.GetoptError:
print(
'aip.py -l <pattern leg> -t <tail number> -c <clear {leg, all}>, -p <pattern {l or r}>\nAcceptable pattern legs are upwind, crosswind, downwind, base, or final'
)
sys.exit(2)
for opt, arg in opts:
if opt == '-h':
print(
'aip.py -l <pattern leg> -t <tail number>\nAcceptable pattern legs are upwind, crosswind, downwind, base, or final'
)
sys.exit()
elif opt in ("-l", "--leg"):
input_leg = arg
elif opt in ("-t", "--tail"):
input_tail = arg
elif opt in ("-c", "--clear"):
clear_oled = arg
elif opt in ("-p", "--pattern"):
pattern_direction = arg
if pattern_direction != "r":
pattern_direction = "l"
# multiplexer index for each OLED
pattern_leg = {}
pattern_leg['downwind'] = 4
if pattern_direction == "l":
pattern_leg['upwind'] = 2
pattern_leg['crosswind'] = 3
pattern_leg['base'] = 5
pattern_leg['final'] = 6
else:
pattern_leg['upwind'] = 6
pattern_leg['crosswind'] = 5
pattern_leg['base'] = 3
pattern_leg['final'] = 2
tail_number = input_tail
leg = input_leg
leg_name = adafruit_ssd1306.SSD1306_I2C(128, 32, tca[pattern_leg[leg]])
if clear_oled == "leg":
leg_name.fill(0)
leg_name.show()
sys.exit()
if clear_oled == "all":
for leg in pattern_leg.keys():
leg_name = adafruit_ssd1306.SSD1306_I2C(128, 32,
tca[pattern_leg[leg]])
leg_name.fill(0)
leg_name.show()
sys.exit()
W, H = (leg_name.width, leg_name.height)
leg_name.fill(0)
leg_name.show()
tail_number_img = Image.new("1", (W, H))
draw_tail_number = ImageDraw.Draw(tail_number_img)
w, h = draw_tail_number.textsize(
tail_number, font=tail_font
) #Figure out the height and width of tail number so we can center it
draw_tail_number.text(((W - w) / 2, (H - h) / 2),
tail_number,
font=tail_font,
fill=255)
offset = 0
# if input_leg == "upwind":
# for i in range(0,16):
# leg_name.scroll(-8,0)
# leg_name.show()
# leg_name.image(tail_number_img)
# leg_name.show()
leg_name.image(tail_number_img)
leg_name.show()
from imusensor.filters import madgwick
from imusensor.filters import kalman
# initializing publisher
host = '192.168.1.8'
port = 8358
url = 'tcp://' + host + ':' + str(port)
context = zmq.Context()
socket = context.socket(zmq.PUB)
socket.bind(url)
sensorfusion = madgwick.Madgwick(0.1)
kalman_filter = kalman.Kalman()
tca = adafruit_tca9548a.TCA9548A(busio.I2C(board.SCL, board.SDA))
address = 0x68
imu = MPU9250.MPU9250(tca, address, 0)
imu.begin()
# imu.caliberateAccelerometer()
# print ("Acceleration calib successful")
# imu.caliberateMag()
# print ("Mag calib successful")
# or load your caliberation file
# imu.loadCalibDataFromFile("/home/pi/calib_real_bolder.json")
imu.readSensor()
imu.computeOrientation()
kalman_filter.roll = imu.roll
kalman_filter.pitch = imu.pitch
kalman_filter.yaw = imu.yaw
def main():
phraseTest = ""
phrase = ""
matriceCouleurs = [[]]
matriceCouleursTest = [["Noir", "Bleu"], ["Rouge", "Marron"], ["Noir", "Noir"], ["Rouge", "Noir"], ["Jaune", "Noir"]]
tabCouleurs = []
liste = []
liste2 = []
# création instance
filin = open("fichierCouleurs.txt", "a")
i2c = busio.I2C(board.SCL, board.SDA)
tca = adafruit_tca9548a.TCA9548A(i2c)
# erreur viens de l'adresse donner en parametres
#tca2 = adafruit_tca9548a.TCA9548A(i2c, 0x71)
#création des capteurs en tableau avec affectation valeurs utilisable
for x in range(0, 2):
liste.append(adafruit_tcs34725.TCS34725(tca[x]))
liste[x].gain = 16
liste[x].integration_time = 200
print("Chaque capteur possède un gain = 16 et un temps d'intégration = 200ms")
# récupération data et test de la couleurs qui lui est lié
for x in range(0, 2):
data = liste[x].color_raw
dataLux = liste[x].lux
print("data0 = ", data)
print("Luminosité0 = ", dataLux)
tabCouleurs.append(testCouleurs(data))
with open("fichierCouleurs.txt", "a") as filout:
filout.write("\n"+str(data)+tabCouleurs[x])
# changement multiplexeur
tca = adafruit_tca9548a.TCA9548A(i2c, 0x71)
for x in range(0, 2):
liste2.append(adafruit_tcs34725.TCS34725(tca[x]))
liste2[x].gain = 16
liste2[x].integration_time = 200
for x in range(0, 2):
data1 = liste2[x].color_raw
dataLux1 = liste2[x].lux
print("data1 = ", data1)
print("Luminosité1 = ", dataLux1)
tabCouleurs.append(testCouleurs(data1))
with open("fichierCouleurs.txt", "a") as filout:
filout.write("\n" + str(data1) + tabCouleurs[x])
print(tabCouleurs)
#remplissage matrice couleurs
matriceCouleurs = [tabCouleurs[i:i+2] for i in range(0, 4, 2)]
# on fait la range avec le pas que l'on souhaite
print(matriceCouleurs)
phrase += testCaractere(matriceCouleurs)
print(phrase)
for x in range(5):
tabCouleurs2 = matriceCouleursTest[x]
phraseTest += testCaractereTest2(tabCouleurs2)
tabCouleurs2 = matriceCouleursTest[0]
print(eval(phraseTest))
def __init__(self, devices_dict, SCL_pin=board.SCL, SDA_pin=board.SDA):
# NOTE: accepting tuples currently because I'm not sure what the config
# will look like yet
# "fn": None --> resort to default fn
self.ALLOWED_DEVICES = {
"vl53l0x": {
"device_class": VL5310X,
"fn": None
},
"si7021": {
"device_class": SI7021,
"fn": None
},
"veml6070": {
"device_class": VEML6070,
"fn": None
},
"pm25": {
"device_class": PM25,
"fn": None
},
"bmp390": {
"device_class": BMP390,
"fn": None
},
}
# connected = (name, address, channel, device, fn)
if devices_dict is None:
raise ValueError("no devices specified in `device_dict`")
# Initialize I2C bus and sensor.
i2c = busio.I2C(SCL_pin, SDA_pin)
# addr_to_tca[addr] = adafruit_tca9548a.TCA9548A(i2c, address=addr)
# TODO: ensure no channel duplicates
# TODO: does it make sense to sort by address for initialization?
addr_to_tca = {}
devices = {}
for name, dd in devices_dict.items():
devices[name] = {}
cur_dev_class = self.ALLOWED_DEVICES[
dd["device_type"]]["device_class"]
if dd["address"] not in addr_to_tca:
addr_to_tca[dd["address"]] = adafruit_tca9548a.TCA9548A(
i2c, address=dd["address"])
cur_tca = addr_to_tca[dd["address"]]
try:
init_params = dd["params"]["init"]
except KeyError:
init_params = {}
# TODO: create channel within device, The device needs to be
# on/active for initialization work as expected
cur_device = cur_dev_class(dd["channel"], cur_tca, **init_params)
devices[name]["device_type"] = cur_device
available_fns = [
f for f in dir(cur_device)
if callable(getattr(cur_device, f)) and not f.startswith("_")
]
try:
dev_fn = dd["fn_name"]
except KeyError:
dev_fn = None
if dev_fn is not None:
if dev_fn not in available_fns:
raise ValueError(
f"specified fn ({dev_fn}) for {name} not available for {cur_device}.\n"
f"please select from {available_fns}")
fn_name = dev_fn
else:
fn_name = "return_value"
try:
devices[name]["fn"] = getattr(devices[name]["device_type"],
fn_name)
except KeyError:
raise ValueError(
f"specified fn ({fn_name}) for {name} not available for {cur_device}.\n"
f"please select from {available_fns}")
self.devices = devices
from time import sleep
from signal import pause
from gpiozero import RGBLED
import board
import busio
import adafruit_tcs34725 # RGB Sensors
import adafruit_tca9548a # Multiplexer
led1 = RGBLED(5,6,13) # RGB LED1 PINS: RED PIN 29, GREEN PIN 31, BLUE PIN 33
led2 = RGBLED(19,26,12) # RGB LED2 PINS: RED PIN 35, GREEN PIN 37, BLUE PIN 32
led3 = RGBLED(16,20,21) # RGB LED3 PINS: RED PIN 36, GREEN PIN 38, BLUE PIN 40
ledArray = [led1,led2,led3]
ledColorCycle = [(1,0,0),(0,1,0),(0,0,1)]
i2c = busio.I2C(board.SCL, board.SDA)
mpx = adafruit_tca9548a.TCA9548A(i2c) # multiplexer
mpx_channels = [3,4,5]
sensorArray = []
ledTest = False
mpxTest = False
rgbSensorTest = True
if mpxTest or rgbSensorTest:
for mpx_channel in mpx_channels:
sensorArray.append(adafruit_tcs34725.TCS34725(mpx[mpx_channel]))
print("Initialized mpx channel",str(mpx_channel),)
def getColorRange(trys):
currentTry = 0
sensorArray = []
import paho.mqtt.client as mqtt
MQTT_SERVER = "192.168.4.1"
MQTT_PATH_COMMAND = "command_channel"
MQTT_PATH_REPLY = "reply_channel"
client = mqtt.Client()
client.on_connect = on_connect
client.on_message = on_message
client.connect(MQTT_SERVER, 1883, 60)
# initialize I2C bus and sensors for mma8451
i2c = busio.I2C(board.SCL, board.SDA)
# create the TCA9548A object and give it the I2C bus
tca = adafruit_tca9548a.TCA9548A(i2c)
# create each sensor using the TCA9548A channel instead of the I2C object
mma8451_1 = adafruit_mma8451.MMA8451(tca[0]) # MMA8451_1
mma8451_2 = adafruit_mma8451.MMA8451(tca[1]) # MMA8451_2
mma8451_3 = adafruit_mma8451.MMA8451(tca[2]) # MMA8451_3
bno055 = adafruit_bno055.BNO055(i2c)
# open files for each sensor
mma8451_1_File = open('/mnt/usb/mma8451_1.txt', 'w')
mma8451_2_File = open('/mnt/usb/mma8451_2.txt', 'w')
mma8451_3_File = open('/mnt/usb/mma8451_3.txt', 'w')
bno055_File = open('/mnt/usb/bno055.txt', 'w')
def data():
def readIMU(q, b, fake_online_data, init_time, signals_per_sensor,
save_dir_init, home_dir):
# Load the initialization information about the sensors
tca_inds = []
num_parts = 0
calibrate_sensors = False
parallelize = False
old_lines = []
save_folder = 'test_dir'
sim_len = 600
# Defining the external signal trigger
imu_only = False
with open(home_dir + 'settings.txt', 'r') as f:
for cnt, line in enumerate(f):
old_lines.append(line)
if cnt == 0:
body_parts = line.split(',')
num_parts = len(body_parts)
elif cnt == 1:
tca_inds = line.split(',')
if num_parts != len(tca_inds):
print(
"Wrong number of tca_indeces given, doesn't match number of body parts."
)
alt_address_list = []
tca_inds = tca_inds[:-1]
for i in range(len(tca_inds)):
if len(tca_inds[i]) == 1: # alternate
tca_inds[i] = int(tca_inds[i])
alt_address_list.append(False)
elif len(tca_inds[i]) > 1:
tca_inds[i] = int(tca_inds[i][0])
alt_address_list.append(True)
elif cnt == 2:
rate = float(line)
print("Rate:", rate)
elif cnt == 7:
cal_word = line.strip()
if cal_word == 'calibrate': # calibrate IMUs at start
calibrate_sensors = True
elif cnt == 6:
sensor_name = line.strip()
elif cnt == 3:
cal_word = line.strip()
if cal_word == 'parallel': # run with extra thread multiprocessing
parallelize = True
fake_real_time = False
elif cal_word == 'online': # run offline with given file path in recordings folder
fake_real_time = False
elif cal_word == 'offline':
fake_real_time = False
imu_only = True
else:
fake_path = cal_word
fake_real_time = True
elif cnt == 4:
cal_word = line.strip()
save_folder = cal_word
elif cnt == 5:
sim_len = float(line)
print("Sim length:", sim_len)
f.close()
if calibrate_sensors:
with open(home_dir + 'settings.txt', 'w') as f:
f.writelines(old_lines[:-1])
f.close()
if not fake_real_time:
from adafruit_lsm6ds import Rate, AccelRange, GyroRange
if sensor_name == 'ISM330DHCX':
from adafruit_lsm6ds import ISM330DHCT as Sensor
elif sensor_name == 'LSM6DS33':
from adafruit_lsm6ds.lsm6ds33 import LSM6DS33 as Sensor
elif sensor_name == 'LSM6DS032':
from adafruit_lsm6ds import LSM6DSOX as Sensor
else:
print(
"An unknown IMU has been specified, please add the text to specify the ISM330DHCX, LSM6DS33, or LSM6DS032 to the second to last line of the settings file (the line before calibrate)."
)
import adafruit_tca9548a
import board
import busio
import digitalio
from micropython import const
from adafruit_bus_device.i2c_device import I2CDevice
trigger = digitalio.DigitalInOut(
board.D16) # external signal should be applied to the BCM 16 pin
trigger.direction = digitalio.Direction.INPUT # this signal will be checked, if 3.3V is applied, recording will be started
trigger.pull = digitalio.Pull.DOWN # pull this input low at all times
trigger_status = False # set to true if the trigger is used to start a recording
# Initializing the different methods
button_address = const(0x6F) # I2c address for LED button
i2c = busio.I2C(board.SCL, board.SDA)
tca = adafruit_tca9548a.TCA9548A(i2c)
button = I2CDevice(tca[tca_inds[0]], button_address)
last_pressed = time.time() - 1.0
pressed = False
button_mode(button, 0) # turn button off
clear_button(button)
# define sensors
sensor_inds = tca_inds[1:]
alt_address_list = alt_address_list[1:]
sensor_list = []
sensor_ind_list = []
sensor_number = []
sensor_cnt = 0
sensor_rot = []
sensor_rot_type = [0, 0, 1, 1, 3, 2, 2, 3, 1, 1, 1, 2, 2,
2] # define rotation types
sensor_labels_full = [
'pelvis_imu', 'torso_imu', 'femur_l_imu', 'tibia_l_imu', 'calcn_l_imu',
'femur_r_imu', 'tibia_r_imu', 'calcn_r_imu', 'humerus_l_imu',
'ulna_l_imu', 'hand_l_imu', 'humerus_r_imu', 'ulna_r_imu', 'hand_r_imu'
]
sensor_label_list = []
for i, s_ind in enumerate(sensor_inds):
if s_ind != 9:
if not fake_real_time:
if alt_address_list[i]: # if true use alternate address
s = Sensor(tca[s_ind], address=const(0x6B))
else:
s = Sensor(tca[s_ind])
sensor_list.append(s)
sensor_ind_list.append(s_ind)
len_sensor_list = len(sensor_ind_list)
sensor_number.append(sensor_cnt)
sensor_cnt += 1
sensor_rot.append(sensor_rot_type[i]
) # say for this number sensor how to rotate it
sensor_label_list.append(sensor_labels_full[i])
# Making the text header for which body segments have IMU data
header_text = 'time\t'
for label in sensor_label_list:
header_text = header_text + '\t' + label
header_text = header_text + '\n'
num_sensors = len(sensor_number)
if not fake_real_time:
for s in sensor_list: # setting all sensors to same default values
s.accelerometer_range = AccelRange.RANGE_8G
s.gyro_range = GyroRange.RANGE_2000_DPS
# To change the imu data sampling frequency, use the lines below. 104 Hz is default.
#s.accelerometer_data_rate = Rate.RATE_416_HZ # Other options: 52_HZ, 26_HZ, 104_HZ, 416_HZ
#s.gyroscope_data_rate = Rate.RATE_416_HZ
# load fake data and figure out number of sensors
quat_cal_offset = int(init_time *
rate) # array for data for calibrating sensors
cwd = os.getcwd() #
sensor_vec = np.zeros(num_sensors * signals_per_sensor)
scaling = np.ones(num_sensors * signals_per_sensor)
offsets = np.zeros(num_sensors * signals_per_sensor)
imu_data = np.zeros((quat_cal_offset, num_sensors * signals_per_sensor))
fake_data_len = 0
if fake_real_time:
cal_data = imu_data
imu_data = np.load(fake_online_data + fake_path) # load fake dataset
cal_data = imu_data[:quat_cal_offset, :]
fake_data_len = imu_data.shape[0]
print("Starting offline analysis for file with", fake_data_len,
"samples")
# calibrating or loading calibration data
if not fake_real_time:
cal_dir = home_dir + 'calibration'
gyro_file = '/gyro_offsets.npy'
if calibrate_sensors or not os.path.exists(
cal_dir): # also check if calibration folder exists
print("Calibrating sensors!")
try: # create calibration dir
os.makedirs(cal_dir)
except:
pass
calibrating_sensors(cal_dir, gyro_file, button, rate, sensor_list)
button_mode(button, 0) # turn button off
offsets = np.load(cal_dir + gyro_file) # loading calibration vec
else:
offsets = 0.0
save_dir = save_dir_init + save_folder + '/' # append the folder name here
file_cnt = 0
try: # create save dir or count number of files so I don't save over some
os.makedirs(save_dir)
except:
f = os.listdir(save_dir)
for s in f:
if 'rec' in s:
file_cnt += 1
b.put([
sensor_number,
rate,
header_text,
parallelize,
save_folder,
file_cnt,
sim_len,
fake_real_time,
fake_data_len,
]) # ready to start running
if fake_real_time:
time.sleep(2.)
for i in range(quat_cal_offset
): # pull in real data and compute quats for init_time
cal_data[i, :] = imu_data[0, :]
Qi, head_err, rot_mats = h.compute_quat(cal_data, len_sensor_list,
quat_cal_offset, sensor_rot,
num_sensors)
q.put([time.time(), Qi, head_err]) # sending initialized info
time_start = time.time()
dt = 1 / rate
madgwick = ahrs.filters.Mahony(frequency=rate)
t = 0
sensor_vec = np.zeros(num_sensors * signals_per_sensor)
start = q.get() # waiting for confirmation of sim Starting
time.sleep(0.3)
while (t < fake_data_len): # Pull data at the desired rate
sensor_vec = imu_data[t, :]
for i in range(len_sensor_list):
s_off = i * signals_per_sensor
accel = np.matmul(sensor_vec[s_off:s_off + 3],
rot_mats[i, :, :])
gyro = np.matmul(sensor_vec[s_off + 3:s_off + 6],
rot_mats[i, :, :])
Qi[i, :] = madgwick.updateIMU(Qi[i, :], gyro, accel)
while (q.qsize() > 0):
time.sleep(0.003)
q.put([time.time(), Qi])
t += 1
b.put([True]) # end the script
else:
while (True): # outer loop for resetting the simulation
button_mode(button, 1) # make button blink
clear_button(button)
while (not pressed): # wait for button press
pressed, last_pressed = check_button(button, last_pressed)
if trigger.value: # External signal pulled up to 3.3V to start recording
pressed = True
trigger_status = True
for i in range(
quat_cal_offset
): # pull in real data and compute quats for init_time
for j, s in enumerate(sensor_list):
s_off = j * signals_per_sensor
imu_data[i, s_off:s_off + 3] = s.acceleration
imu_data[i, s_off + 3:s_off +
6] = s.gyro + offsets[s_off + 3:s_off + 6]
imu_data[i, :] = imu_data[i, :] + offsets # correcting gyro bias
Qi, head_err, rot_mats = h.compute_quat(imu_data, len_sensor_list,
quat_cal_offset,
sensor_rot, num_sensors)
q.put([time.time(), Qi, head_err]) # sending initialized info
time_start = time.time()
dt = 1 / rate
madgwick = ahrs.filters.Mahony(frequency=rate)
t = 0
sensor_vec = np.zeros(num_sensors * signals_per_sensor)
sensor_mat = np.zeros(
(int(sim_len * rate), num_sensors * signals_per_sensor))
start = q.get() # waiting for confirmation of sim Starting
time.sleep(0.3)
button_mode(button, 2) # make button solid red to start recording
clear_button(button)
while (True): # Pull data at the desired rate
cur_time = time.time()
if cur_time >= time_start + dt: # time for next reading
pressed, last_pressed = check_button(button, last_pressed)
if trigger_status: # trigger was engaged
if not trigger.value: # the external signal is now low (0V), stop recording
pressed = True
if pressed or (b.qsize() >
0): # send message to exit the recording
b.put([pressed])
q.put([cur_time, Qi])
button_mode(button, 0) # turn button off
np.save(save_dir + 'raw_imu_' + str(file_cnt) + '.npy',
sensor_mat[:t, :]) # saving kinematics
file_cnt += 1
pressed = False
time.sleep(1.0)
break
time_start = cur_time
for j, s in enumerate(sensor_list):
s_off = j * signals_per_sensor
sensor_vec[s_off:s_off + 3] = s.acceleration
sensor_vec[s_off + 3:s_off + 6] = s.gyro
sensor_vec = sensor_vec + offsets # preping
sensor_mat[t, :] = sensor_vec
for i in range(len(sensor_list)):
s_off = i * signals_per_sensor
accel = np.matmul(sensor_vec[s_off:s_off + 3],
rot_mats[i, :, :])
gyro = np.matmul(sensor_vec[s_off + 3:s_off + 6],
rot_mats[i, :, :])
Qi[i, :] = madgwick.updateIMU(Qi[i, :], gyro, accel)
if not imu_only:
q.put([cur_time, Qi])
t += 1