jumeaux-numeriques/portail_coulissant/porcou.py

399 lines
18 KiB
Python
Raw Blame History

This file contains invisible Unicode characters

This file contains invisible Unicode characters that are indistinguishable to humans but may be processed differently by a computer. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

import bge # Bibliothèque Blender Game Engine (UPBGE)
import twin # Bibliothèque de l'environnement 3D des jumeaux numériques
import math
import time
###############################################################################
# porcou.py
# @title: Commandes pour le portail coulissant
# @project: Blender-EduTech
# @lang: fr
# @authors: Philippe Roy <philippe.roy@ac-grenoble.fr>
# @copyright: Copyright (C) 2020-2023 Philippe Roy
# @license: GNU GPL
###############################################################################
# Récupérer la scène UPBGE
scene = bge.logic.getCurrentScene()
# Configuration des variables publiques
# 'nom_variable' :
# - Objet 3D : [nom de l'objet 3D, propriété du stockage de la valeur (activate ou activated_real)]
# - Configuration de la broche : [nom de la propriété stockant l'object broche (pyfirmata), type de broche par défaut('d','a' ou 'p'), 'mode de la broche par défaut ('i' ou 'o')]
# - Configuration du graphique : ['marque', 'type de ligne', 'couleur', linewidth]] (matplotlib)
#
# 'nom_variable_r' est la valeur réelle de la variable (valeur numérique) 'nom_variable' issue du jumelage numérique.
# Dans ce cas, il n'y a pas configuration de broche car elle est présente sur la variable 'nom_variable'.
# Ce distinguo ne concerne que les entrées, les sorties réelles sont forcées par les sorties numériques et vice-versa.
public_vars = {
'bp_ext' : [['Bp cote rue','activated'], ['pin', 'd','i'], ['o','-', 'green', 1]],
'bp_ext_r' : [['Bp cote rue','activated_real'], [], ['o','--', 'green', 1]],
'bp_int' : [['Bp cote cour','activated'], ['pin', 'd','i'], ['o','-', 'darkgreen', 1]],
'bp_int_r' : [['Bp cote cour','activated_real'], [], ['o','--', 'darkgreen', 1]],
'fdc_o' : [['Microrupteur fdc ouvert','activated'], ['pin', 'd','i'], ['o','-', 'orange', 1]],
'fdc_o_r' : [['Microrupteur fdc ouvert','activated_real'], [], ['o','--', 'orange', 1]],
'fdc_f' : [['Microrupteur fdc ferme','activated'], ['pin', 'd','i'], ['o','-', 'darkorange', 1]],
'fdc_f_r' : [['Microrupteur fdc ferme','activated_real'], [], ['o','--', 'darkorange', 1]],
'mot_o' : [['Moteur','open'], ['pin_open', 'd','o'], ['o','-', 'violet', 1]],
'mot_f' : [['Moteur','close'], ['pin_close', 'd','o'], ['o','-', 'darkviolet', 1]],
'mot_angle' : [['Moteur','alpha'], [], ['o','-', 'blue', 1]],
'mot_vitesse' : [['Moteur','omega'], [], ['o','-', 'darkblue', 1]],
'portail_x' : [['Portail','x'], [], ['o','-', 'turquoise', 1]],
'gyr' : [['Led','activated'], ['pin', 'd','o'], ['o','-', 'gold', 1]],
'ir_emet' : [['Emetteur IR', 'activated'], ['pin', 'd','o'],['o','-', 'red', 1]],
'ir_recep' : [['Recepteur IR','activated'], ['pin', 'd','i'],['o','-', 'darkred', 1]],
'ir_recep_r' : [['Recepteur IR','activated_real'], [],['o','--', 'darkred', 1]]}
# Couleurs
color_passive = (0.800, 0.005, 0.315,1) # bouton non activable : magenta
color_active = (0.799, 0.130, 0.063,1) # bouton activable : orange
color_hl = (0.8, 0.8, 0.8, 1) # bouton focus : blanc
color_activated = (0.8, 0.619, 0.021, 1) # bouton activé numériquement uniquement : jaune
color_activated_real = (0.799, 0.031, 0.038, 1) # élément activé physiquement uniquement : rouge (hors clic)
color_activated_dbl = (0.246, 0.687, 0.078, 1) # élément activé physiquement et numériquement : vert clair
# Constantes UPBGE
JUST_ACTIVATED = bge.logic.KX_INPUT_JUST_ACTIVATED
JUST_RELEASED = bge.logic.KX_INPUT_JUST_RELEASED
ACTIVATE = bge.logic.KX_INPUT_ACTIVE
# JUST_DEACTIVATED = bge.logic.KX_SENSOR_JUST_DEACTIVATED
###############################################################################
# Initialisation de la scène
###############################################################################
def init(cont):
if cont.sensors['Init'].positive == False: # 1 seule fois
return False
twin.manip_init() # Manipulation du modèle 3D
twin.cmd_init() # Commandes
# Brochage
for pin in public_vars:
if public_vars[pin][1] != []:
scene.objects[public_vars[pin][0][0]][public_vars[pin][1][0]] = None
# Mémorisation de la position et orientation des composants du système
scene.objects['Portail']['init_lx']=scene.objects['Portail'].worldPosition.x
scene.objects['Portail']['init_ly']=scene.objects['Portail'].worldPosition.y
scene.objects['Portail']['init_lz']=scene.objects['Portail'].worldPosition.z
scene.objects['Engrenage']['init_rx']=scene.objects['Engrenage'].worldOrientation.to_euler().x
scene.objects['Engrenage']['init_ry']=scene.objects['Engrenage'].worldOrientation.to_euler().y
scene.objects['Engrenage']['init_rz']=scene.objects['Engrenage'].worldOrientation.to_euler().z
# Groupe de focus pour les actionneurs
twin.cycle_def_focusgroup([["Moteur","blue"] ,
["Reducteur","blue"],
["Pattes moteur","blue"],
["Engrenage","blue-dark"],
["Engrennage patte","blue-dark"],
["Engrenage vis1","grey"],
["Engrenage vis2","grey"],
["Engrenage vis3","grey"]], "Moteur : mot_o(True | False), mot_f(True | False)")
twin.cycle_def_focusgroup([["Led", "led_yellow"]], "Gyrophare : gyr(True | False)")
# Focus sur les boutons et capteurs
scene.objects['Bp cote cour']['description']="Bouton poussoir coté cour : bp_int()"
scene.objects['Bp cote rue']['description']="Bouton poussoir coté rue : bp_ext()"
scene.objects['Microrupteur fdc ouvert']['description']="Capteur fin de course portail ouvert : fdc_o()"
scene.objects['Microrupteur fdc ferme']['description']="Capteur fin de course portail fermé : fdc_f()"
scene.objects['Emetteur IR']['description']="Capteur barrage IR (absence d\"obstacle) : ir_recep()"
scene.objects['Recepteur IR']['description']="Capteur barrage IR (absence d\"obstacle) : ir_recep()"
system_init() # Initialisation du système
scene.objects['System']['plot_proc'] = None # Initialisation du processus plot
def get_public_vars():
return public_vars
###############################################################################
# Actionneurs
###############################################################################
##
# Moteur et portail
##
def mot (cont):
if scene.objects['System']['run']:
obj = cont.owner
vitesse = 0.05 # valeur empirique
pas_portail = 2.35619/0.3 # pas echelle 1:1 = 2.35619 -> pas à l'échelle de la maquette (0,3) : 2.35619/0.3 = 7,85396 valeur empirique
pas_engrenage = math.pi/7 # z = 14 valeur empirique
obj_engrenage = scene.objects['Engrenage']
obj_portail = scene.objects['Portail']
# # obj_portail['x']= scene.objects['Portail'].localPosition.x # Affichage de la position du portail
# speed= obj['speed'] # 0,5 en tour / seconde -> course de 500 mm en 4s
# speed= 0.1
# scene.objects['System']['scale']=0.33
# # speed= obj['speed'] # 0,2 en tour / seconde -> course de 500 mm en 20s
# gear_radius = ((obj_engrenage['diameter'])/2) *(1/scene.objects['System']['scale']) # diametre : 40 mm et scale : 0,0033
# if obj['close']:
# print ("speed :", speed)
# print ("gear_radius :", gear_radius)
# print ("Vitesse engrenage : ", speed * 2 * math.pi*(1/60), pas_engrenage*vitesse)
# print ("Vitesse portail : ", speed * 2 * math.pi * gear_radius*(1/60), pas_portail*vitesse)
# print ("Ratio Vitesse portail/ Vitesse engrenage : ",( speed * 2 * math.pi * gear_radius*(1/60))/(speed * 2 * math.pi *(1/60)), pas_portail/pas_engrenage)
# Ouvrir
if obj['open']:
# obj_engrenage.applyRotation((0, 0, -speed * 2 * math.pi), True)
# obj_portail.applyMovement((-speed * math.pi * gear_diameter, 0, 0), True)
obj_engrenage.applyRotation((0, 0, -pas_engrenage*vitesse), True)
obj_portail.applyMovement((-pas_portail*vitesse, 0, 0), True)
# Modele 3D -> Arduino
if scene.objects['System']['twins']:
if scene.objects['Moteur']['pin_o'] is not None:
if scene.objects['Moteur']['pin_f'] is not None:
scene.objects['Moteur']['pin_f'].write(0)
scene.objects['Moteur']['pin_o'].write(1)
# Fermer
# else: # Pas de priorité
if obj['close']:
# obj_engrenage.applyRotation((0, 0, speed * 2 * math.pi*(1/60)), True)
# obj_portail.applyMovement((speed * 2 * math.pi * gear_radius*(1/60), 0, 0), True)
obj_engrenage.applyRotation((0, 0, pas_engrenage*vitesse), True)
obj_portail.applyMovement((pas_portail*vitesse, 0, 0), True)
# Modele 3D -> Arduino
if scene.objects['System']['twins']:
if scene.objects['Moteur']['pin_f'] is not None:
if scene.objects['Moteur']['pin_o'] is not None:
scene.objects['Moteur']['pin_o'].write(0)
scene.objects['Moteur']['pin_f'].write(1)
# Arrêrer
if obj['open']== False and obj['close'] == False :
# Modele 3D -> Arduino
if scene.objects['System']['twins']:
if scene.objects['Moteur']['pin_f'] is not None:
if scene.objects['Moteur']['pin_o'] is not None:
scene.objects['Moteur']['pin_o'].write(0)
scene.objects['Moteur']['pin_f'].write(0)
###############################################################################
# Capteurs fin de course
###############################################################################
##
# Etat capteur fin de course portail ouvert
##
def fdc_o (cont):
if scene.objects['System']['run'] :
obj = cont.owner
# Arduino -> Modele 3D
if scene.objects['System']['twins']:
if obj['pin'] is not None:
if obj['pin'].read()==True and obj['activated_real'] == False :
obj['activated_real'] = True
if obj['pin'].read()==False and obj['activated_real'] == True :
obj['activated_real'] = False
# Etat capteur en fonction de la grille : worldPosition.x : 0 -> 65.5 et localPosition.x : 0 -> 218
if scene.objects['Portail'].localPosition.x <= 0 and obj['activated'] == False :
obj['activated'] = True
if scene.objects['Portail'].localPosition.x > 0 and obj['activated'] == True :
obj['activated'] = False
# Forçage par clic
if obj['click'] == True:
obj['activated'] = True
# Couleurs
twin.cycle_sensitive_color(obj)
##
# Etat capteur fin de course portail fermé
##
def fdc_f (cont):
if scene.objects['System']['run'] :
obj = cont.owner
# Arduino -> Modele 3D
if scene.objects['System']['twins']:
if obj['pin'] is not None:
if obj['pin'].read()==True and obj['activated_real'] == False :
obj['activated_real'] = True
if obj['pin'].read()==False and obj['activated_real'] == True :
obj['activated_real'] = False
# Etat capteur en fonction de la grille : worldPosition.x : 0 -> 65.5 et localPosition.x : 0 -> 218
if scene.objects['Portail'].localPosition.x >= 218 and obj['activated'] == False :
obj['activated'] = True
if scene.objects['Portail'].localPosition.x < 218 and obj['activated'] == True :
obj['activated'] = False
# Forçage par clic
if obj['click'] == True:
obj['activated'] = True
# Couleurs
twin.cycle_sensitive_color(obj)
###############################################################################
# Capteur barrage
###############################################################################
##
# Emetteur IR
##
def ir_emet (cont):
if scene.objects['System']['run'] :
obj = cont.owner
# Mouse over
if obj['mo'] == True and obj['click'] == False and obj.color !=color_hl:
obj.color =color_hl
return
# Passif
if obj['active'] == False and obj.color !=color_passive:
obj.color =color_passive
scene.objects['Emetteur IR Led'].setVisible(True,False)
scene.objects['Emetteur IR Led-on'].setVisible(False,False)
scene.objects['Recepteur IR']['active'] = False
# Modele 3D -> Arduino
if scene.objects['System']['twins']:
if scene.objects['Emetteur IR']['pin'] is not None:
scene.objects['Emetteur IR']['pin'].write(0)
return
# Active
if obj['active']:
# Allumage
if scene.objects['Emetteur IR Led-on'].visible == False:
scene.objects['Emetteur IR Led-on'].setVisible(True,False)
scene.objects['Emetteur IR Led'].setVisible(False,False)
obj.color = color_active
scene.objects['Recepteur IR']['active'] = True
# Modele 3D -> Arduino
if scene.objects['System']['twins']:
if scene.objects['Emetteur IR']['pin'] is not None:
scene.objects['Emetteur IR']['pin'].write(1)
# Forçage par clic
if obj['click'] == True:
obj['activated'] = True
scene.objects['Recepteur IR']['activated'] = True
# Couleurs
# FIXME : à faire
if obj['activated'] == True and obj.color !=color_activated:
obj.color =color_activated
if obj['activated'] == False :
if obj['mo'] == True and obj.color !=color_hl:
obj.color =color_hl
if obj['mo'] == False and obj.color !=color_active:
obj.color =color_active
##
# Récepteur IR
# FIXME : Modele 3D -> Arduino à faire
##
def ir_recep (cont):
if scene.objects['System']['run'] :
obj = cont.owner
# Mouse over
if obj['mo'] == True and obj['click'] == False and obj.color !=color_hl:
obj.color =color_hl
return
# Passif
if obj['active'] == False and obj.color !=color_passive:
obj.color =color_passive
return
# Active
if obj['active']:
# Forçage par clic
if obj['click'] == True:
obj['activated'] = True
scene.objects['Emetteur IR']['activated'] = True
# Couleurs
# FIXME : à faire
if obj['activated'] == True and obj.color !=color_activated:
obj.color =color_activated
if obj['activated'] == False :
if obj['mo'] == True and obj.color !=color_hl:
obj.color =color_hl
if obj['mo'] == False and obj.color !=color_active:
obj.color =color_active
###############################################################################
# Système
###############################################################################
##
# Initialisation du système
##
def system_init ():
system_reset()
##
# Réinitialisation du système
##
def system_reset ():
# Grille à l'état initial
scene.objects['Portail'].worldPosition.x = scene.objects['Portail']['init_lx']-scene.objects['System']['init_lx']+scene.objects['System'].worldPosition.x
scene.objects['Portail'].worldPosition.y = scene.objects['Portail']['init_ly']-scene.objects['System']['init_ly']+scene.objects['System'].worldPosition.y
scene.objects['Portail'].worldPosition.z = scene.objects['Portail']['init_lz']-scene.objects['System']['init_lz']+scene.objects['System'].worldPosition.z
# Moteur à l'état initial
rres=0.001 # resolution rotation
obj1=scene.objects['Engrenage']
while (obj1.localOrientation.to_euler().y) > 1.1*rres :
obj1.applyRotation((0, 0, -rres), True)
while (obj1.localOrientation.to_euler().y) < -1.1*rres :
obj1.applyRotation((0, 0, rres), True)
# Gyrophare à l'état initial
scene.objects['Led'].setVisible(True,False)
scene.objects['Led-on'].setVisible(False,False)
# Capteur barrage IR
scene.objects['Emetteur IR Led'].setVisible(True,False)
scene.objects['Emetteur IR Led-on'].setVisible(False,False)
scene.objects['Emetteur IR'].color = color_passive
scene.objects['Recepteur IR'].color = color_passive
# I/O à l'état initial
scene.objects['Led']['activated']=False
scene.objects['Moteur']['open']=False
scene.objects['Moteur']['close']=False
scene.objects['Microrupteur fdc ouvert']['activated']=False
scene.objects['Microrupteur fdc ouvert']['activated_real']=False
scene.objects['Microrupteur fdc ferme']['activated']=False
scene.objects['Microrupteur fdc ferme']['activated_real']=False
scene.objects['Bp cote cour']['activated'] =False
scene.objects['Bp cote cour']['activated_real'] =False
scene.objects['Bp cote rue']['activated'] =False
scene.objects['Bp cote rue']['activated_real'] =False
scene.objects['Emetteur IR']['activated'] =False
scene.objects['Emetteur IR']['activated_real'] =False
scene.objects['Emetteur IR']['active'] =False
scene.objects['Recepteur IR']['activated'] =False
scene.objects['Recepteur IR']['active'] =False
scene.objects['Recepteur IR']['activated_real'] =True # Absence d'obstacle -> True, présence d'obstacle -> False