mirror of
https://forge.apps.education.fr/blender-edutech/jumeaux-numeriques.git
synced 2024-01-27 06:56:18 +01:00
306 lines
12 KiB
Python
306 lines
12 KiB
Python
|
import bge # Bibliothèque Blender Game Engine (UPBGE)
|
|||
|
|
import twin # Bibliothèque de l'environnement 3D des jumeaux numériques
|
|||
|
|
import math
|
|||
|
|
import time
|
|||
|
|
|
|||
|
|
###############################################################################
|
|||
|
|
# ergojr.py
|
|||
|
|
# @title: Commandes pour le bras robotisé Poppy Ergo Jr
|
|||
|
|
# @project: Blender-EduTech
|
|||
|
|
# @lang: fr
|
|||
|
|
# @authors: Philippe Roy <philippe.roy@ac-grenoble.fr>
|
|||
|
|
# @copyright: Copyright (C) 2022 Philippe Roy
|
|||
|
|
# @license: GNU GPL
|
|||
|
|
#
|
|||
|
|
# Commandes déclenchées par/sur le simulateur (sml_*)
|
|||
|
|
###############################################################################
|
|||
|
|
|
|||
|
|
# Récupérer la scène UPBGE
|
|||
|
|
scene = bge.logic.getCurrentScene()
|
|||
|
|
|
|||
|
|
# Couleurs
|
|||
|
|
|
|||
|
|
couleur_magenta = (0.800, 0.005, 0.315,1) # bouton non activable : magenta
|
|||
|
|
couleur_orange = (0.799, 0.130, 0.063,1) # bouton activable : orange
|
|||
|
|
couleur_blanc = (0.8, 0.8, 0.8, 1) # bouton focus : blanc
|
|||
|
|
couleur_jaune = (0.8, 0.619, 0.021, 1) # bouton activé : jaune
|
|||
|
|
|
|||
|
|
# 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
|
|||
|
|
|
|||
|
|
# 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
|
|||
|
|
|
|||
|
|
system_init() # Initialisation du système
|
|||
|
|
|
|||
|
|
###############################################################################
|
|||
|
|
# Actionneurs
|
|||
|
|
###############################################################################
|
|||
|
|
|
|||
|
|
##
|
|||
|
|
# Action du simulateur pour le clignotant
|
|||
|
|
##
|
|||
|
|
|
|||
|
|
def sml_clignotant (cont):
|
|||
|
|
if scene.objects['System']['run']:
|
|||
|
|
obj = cont.owner
|
|||
|
|
if obj['actif'] and scene.objects['Led allumee'].visible == False:
|
|||
|
|
scene.objects['Led allumee'].setVisible(True,False)
|
|||
|
|
scene.objects['Led'].setVisible(False,False)
|
|||
|
|
# print ("Clignotant allumée")
|
|||
|
|
if obj['actif']==False and scene.objects['Led allumee'].visible == True:
|
|||
|
|
scene.objects['Led'].setVisible(True,False)
|
|||
|
|
scene.objects['Led allumee'].setVisible(False,False)
|
|||
|
|
# print ("Clignotant éteint")
|
|||
|
|
|
|||
|
|
##
|
|||
|
|
# Action du simulateur pour le moteur
|
|||
|
|
##
|
|||
|
|
|
|||
|
|
def sml_moteur (cont):
|
|||
|
|
if scene.objects['System']['run']:
|
|||
|
|
obj = cont.owner
|
|||
|
|
pas_rot = math.pi/7 # z = 14
|
|||
|
|
pas = 2.35619/0.3 # pas echelle 1:1 = 2.35619 -> pas à l'échelle de la maquette (0,3) : 2.35619/0.3 = 7,85396
|
|||
|
|
vitesse = 0.05
|
|||
|
|
engrenage_obj = scene.objects['Engrenage']
|
|||
|
|
portail_obj = scene.objects['Portail']
|
|||
|
|
if obj['actif_ouvrir']:
|
|||
|
|
# print (scene.objects['Portail'].worldPosition.x)
|
|||
|
|
engrenage_obj.applyRotation((0, 0, -pas_rot*vitesse), True)
|
|||
|
|
portail_obj.applyMovement((-pas*vitesse, 0, 0), True)
|
|||
|
|
# else:
|
|||
|
|
if obj['actif_fermer']:
|
|||
|
|
# print (scene.objects['Portail'].worldPosition.x)
|
|||
|
|
engrenage_obj.applyRotation((0, 0, pas_rot*vitesse), True)
|
|||
|
|
portail_obj.applyMovement((pas*vitesse, 0, 0), True)
|
|||
|
|
|
|||
|
|
###############################################################################
|
|||
|
|
# Capteurs fin de course
|
|||
|
|
###############################################################################
|
|||
|
|
|
|||
|
|
##
|
|||
|
|
# Etat capteur fin de course portail ouvert
|
|||
|
|
##
|
|||
|
|
|
|||
|
|
def sml_fdc_ouvert (cont):
|
|||
|
|
if scene.objects['System']['run'] :
|
|||
|
|
obj = cont.owner
|
|||
|
|
obj_etat=obj['actif']
|
|||
|
|
obj_microrupteur=scene.objects['Microrupteur fdc ouvert']
|
|||
|
|
# 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['actif'] == False :
|
|||
|
|
obj['actif'] = True
|
|||
|
|
if scene.objects['Portail'].localPosition.x > 0 and obj_microrupteur['actif'] == False and obj['actif'] == True :
|
|||
|
|
obj['actif'] = False
|
|||
|
|
#Forçage
|
|||
|
|
if obj_microrupteur['actif'] == True:
|
|||
|
|
obj['actif'] = True
|
|||
|
|
#Couleurs
|
|||
|
|
if obj['actif'] == True and obj_microrupteur.color !=couleur_jaune:
|
|||
|
|
obj_microrupteur.color =couleur_jaune
|
|||
|
|
if obj['actif'] == False :
|
|||
|
|
if obj_microrupteur['MO'] == True and obj_microrupteur.color !=couleur_blanc:
|
|||
|
|
obj_microrupteur.color =couleur_blanc
|
|||
|
|
if obj_microrupteur['MO'] == False and obj_microrupteur.color !=couleur_orange:
|
|||
|
|
obj_microrupteur.color =couleur_orange
|
|||
|
|
|
|||
|
|
##
|
|||
|
|
# Etat capteur fin de course portail fermé
|
|||
|
|
##
|
|||
|
|
|
|||
|
|
def sml_fdc_ferme (cont):
|
|||
|
|
if scene.objects['System']['run'] :
|
|||
|
|
obj = cont.owner
|
|||
|
|
obj_etat=obj['actif']
|
|||
|
|
obj_microrupteur=scene.objects['Microrupteur fdc ferme']
|
|||
|
|
# 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['actif'] == False :
|
|||
|
|
obj['actif'] = True
|
|||
|
|
if scene.objects['Portail'].localPosition.x < 218 and obj_microrupteur['actif'] == False and obj['actif'] == True :
|
|||
|
|
obj['actif'] = False
|
|||
|
|
#Forçage
|
|||
|
|
if obj_microrupteur['actif'] == True:
|
|||
|
|
obj['actif'] = True
|
|||
|
|
#Couleurs
|
|||
|
|
if obj['actif'] == True and obj_microrupteur.color !=couleur_jaune:
|
|||
|
|
obj_microrupteur.color =couleur_jaune
|
|||
|
|
if obj['actif'] == False :
|
|||
|
|
if obj_microrupteur['MO'] == True and obj_microrupteur.color !=couleur_blanc:
|
|||
|
|
obj_microrupteur.color =couleur_blanc
|
|||
|
|
if obj_microrupteur['MO'] == False and obj_microrupteur.color !=couleur_orange:
|
|||
|
|
obj_microrupteur.color =couleur_orange
|
|||
|
|
|
|||
|
|
##
|
|||
|
|
# Forçage capteur fin de course avec la souris
|
|||
|
|
##
|
|||
|
|
|
|||
|
|
def sml_microrupteur (cont):
|
|||
|
|
if scene.objects['System']['run'] :
|
|||
|
|
system_click(cont, cont.owner)
|
|||
|
|
|
|||
|
|
###############################################################################
|
|||
|
|
# Capteur barrage
|
|||
|
|
###############################################################################
|
|||
|
|
|
|||
|
|
##
|
|||
|
|
# Emetteur IR
|
|||
|
|
##
|
|||
|
|
|
|||
|
|
def sml_emet_ir (cont):
|
|||
|
|
if scene.objects['System']['run'] :
|
|||
|
|
obj = cont.owner
|
|||
|
|
obj_emetteur_ir=scene.objects['Emetteur IR']
|
|||
|
|
obj_recepteur_ir=scene.objects['Recepteur IR']
|
|||
|
|
if obj['actif'] and scene.objects['Emetteur IR Led allumee'].visible == False:
|
|||
|
|
scene.objects['Emetteur IR Led allumee'].setVisible(True,False)
|
|||
|
|
scene.objects['Emetteur IR Led'].setVisible(False,False)
|
|||
|
|
obj_emetteur_ir.color = couleur_orange
|
|||
|
|
obj_recepteur_ir.color = couleur_orange
|
|||
|
|
if obj['actif']==False and scene.objects['Emetteur IR Led allumee'].visible == True:
|
|||
|
|
scene.objects['Emetteur IR Led'].setVisible(True,False)
|
|||
|
|
scene.objects['Emetteur IR Led allumee'].setVisible(False,False)
|
|||
|
|
obj_emetteur_ir.color = couleur_magenta
|
|||
|
|
obj_recepteur_ir.color = couleur_magenta
|
|||
|
|
|
|||
|
|
##
|
|||
|
|
# Récepteur IR
|
|||
|
|
##
|
|||
|
|
|
|||
|
|
def sml_recep_ir (cont):
|
|||
|
|
if scene.objects['System']['run'] and scene.objects['Module emetteur IR']['actif'] :
|
|||
|
|
obj = cont.owner
|
|||
|
|
system_click(cont, scene.objects['Module recepteur IR'])
|
|||
|
|
if scene.objects['Module recepteur IR']['actif']==True :
|
|||
|
|
scene.objects['Emetteur IR'].color = couleur_jaune
|
|||
|
|
scene.objects['Recepteur IR'].color = couleur_jaune
|
|||
|
|
|
|||
|
|
###############################################################################
|
|||
|
|
# Boutons poussoirs
|
|||
|
|
###############################################################################
|
|||
|
|
|
|||
|
|
##
|
|||
|
|
# Bouton pousssoir coté rue
|
|||
|
|
##
|
|||
|
|
|
|||
|
|
def sml_bp_rue (cont):
|
|||
|
|
if scene.objects['System']['run'] :
|
|||
|
|
system_click(cont, scene.objects['Module bouton cote rue'])
|
|||
|
|
|
|||
|
|
##
|
|||
|
|
# Bouton pousssoir coté cour
|
|||
|
|
##
|
|||
|
|
|
|||
|
|
def sml_bp_cour (cont):
|
|||
|
|
if scene.objects['System']['run'] :
|
|||
|
|
system_click(cont, scene.objects['Module bouton cote cour'])
|
|||
|
|
|
|||
|
|
###############################################################################
|
|||
|
|
# Système
|
|||
|
|
###############################################################################
|
|||
|
|
|
|||
|
|
##
|
|||
|
|
# Initialisation du système
|
|||
|
|
# Le moteur est géré en continue.
|
|||
|
|
##
|
|||
|
|
|
|||
|
|
def system_init ():
|
|||
|
|
|
|||
|
|
# Clignotant
|
|||
|
|
scene.objects['Module led']['actif']=False
|
|||
|
|
scene.objects['Led allumee'].setVisible(False,False)
|
|||
|
|
scene.objects['Led'].setVisible(True,False)
|
|||
|
|
|
|||
|
|
# Emetteur IR
|
|||
|
|
scene.objects['Module emetteur IR']['actif']=False
|
|||
|
|
scene.objects['Emetteur IR Led allumee'].setVisible(False,False)
|
|||
|
|
scene.objects['Emetteur IR Led'].setVisible(True,False)
|
|||
|
|
scene.objects['Emetteur IR'].color = couleur_magenta
|
|||
|
|
scene.objects['Recepteur IR'].color = couleur_magenta
|
|||
|
|
|
|||
|
|
##
|
|||
|
|
# Réinitialisation du système
|
|||
|
|
##
|
|||
|
|
|
|||
|
|
def system_reset ():
|
|||
|
|
|
|||
|
|
# Grille à l'état initial
|
|||
|
|
# applyRotationTo(scene.objects['System'], 0, 0, 0)
|
|||
|
|
scene.objects['Portail'].worldPosition.x = scene.objects['Portail']['init_lx']
|
|||
|
|
scene.objects['Portail'].worldPosition.y = scene.objects['Portail']['init_ly']
|
|||
|
|
scene.objects['Portail'].worldPosition.z = scene.objects['Portail']['init_lz']
|
|||
|
|
|
|||
|
|
# 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)
|
|||
|
|
|
|||
|
|
# Led à l'état initial
|
|||
|
|
scene.objects['Led'].setVisible(True,False)
|
|||
|
|
scene.objects['Led allumee'].setVisible(False,False)
|
|||
|
|
|
|||
|
|
# Capteur barrage IR
|
|||
|
|
scene.objects['Emetteur IR Led'].setVisible(True,False)
|
|||
|
|
scene.objects['Emetteur IR Led allumee'].setVisible(False,False)
|
|||
|
|
scene.objects['Emetteur IR'].color = couleur_magenta
|
|||
|
|
scene.objects['Recepteur IR'].color = couleur_magenta
|
|||
|
|
|
|||
|
|
# I/O à l'état initial
|
|||
|
|
scene.objects['Module led']['actif']=False
|
|||
|
|
scene.objects['Ensemble moteur']['actif_ouvrir']=False
|
|||
|
|
scene.objects['Ensemble moteur']['actif_fermer']=False
|
|||
|
|
scene.objects['Capteur fdc ouvert']['actif'] =True
|
|||
|
|
scene.objects['Capteur fdc ferme']['actif'] =False
|
|||
|
|
scene.objects['Module emetteur IR']['actif'] =False
|
|||
|
|
scene.objects['Module recepteur IR']['actif'] =False
|
|||
|
|
scene.objects['Module bouton cote rue']['actif'] =False
|
|||
|
|
scene.objects['Module bouton cote cour']['actif'] =False
|
|||
|
|
|
|||
|
|
##
|
|||
|
|
# Click sur les éléments cliquables du systèmes
|
|||
|
|
##
|
|||
|
|
|
|||
|
|
def system_click(cont, obj_activation):
|
|||
|
|
obj = cont.owner
|
|||
|
|
if cont.sensors['MO'].status == JUST_ACTIVATED :
|
|||
|
|
obj.color = couleur_blanc
|
|||
|
|
obj['MO']=True
|
|||
|
|
if cont.sensors['Click'].status == JUST_ACTIVATED and scene.objects['System']['manip_mode']==0:
|
|||
|
|
if obj['MO']:
|
|||
|
|
obj_activation['actif'] = True
|
|||
|
|
obj.color = couleur_jaune
|
|||
|
|
if cont.sensors['MO'].status == JUST_RELEASED :
|
|||
|
|
obj['MO']=False
|
|||
|
|
if cont.sensors['Click'].status == ACTIVATE :
|
|||
|
|
obj.color = couleur_jaune
|
|||
|
|
else:
|
|||
|
|
obj.color = couleur_orange
|
|||
|
|
if cont.sensors['Click'].status == JUST_RELEASED:
|
|||
|
|
obj_activation['actif'] = False
|
|||
|
|
obj.color = couleur_blanc
|
|||
|
|
if cont.sensors['MO'].status != ACTIVATE :
|
|||
|
|
obj.color = couleur_orange
|