En collaboration avec Graph100, voici un prototype de bras robot utilisant le moteur physique de PB. Le contrôle n'est pas très pratique (Graph100 a produit une variante avec des sliders à la place des champs de saisie, pour ceux qui préfèrent), mais bon, c'est juste une "démo de concept".
Petit (?) souci: les hingeJoints et les moteurs ne sont pas très rigides, ce qui fait que les différentes parties du bras ont tendance à retomber sous l'effet de la gravité plutôt que de rester dans la position voulue....
Si vous avez une solution, nous sommes preneurs !
Code : Tout sélectionner
; Press "W" to toggle wireframe display
; Press "P" to toggle physics display
; Window size
#SCREENWIDTH = 1000
#SCREENHEIGHT = 500
Enumeration
#BASE
#ARM1
#ARM2
#ARM3
#ARM4
#FINGER1
#FINGER2
#END_SEGMENTLIST
EndEnumeration
Structure roboPart_struct
segment.i ; entity
motor.i ; n° hingeJoint
motorSpeed.f
minRotation.f
maxRotation.f
oldAngle.f
targetAngle.f
timeBeforeStop.f
startMvtTimer.f
EndStructure
Global Dim roboPart.roboPart_struct(#END_SEGMENTLIST)
Global wireframe.b, viewBodies.b, simRunning.b = #True, notQuit.b = #True, eventId.i
Global txBlank.i, txGround.i
Global mtGround.i, mtRed.i, mtBlue.i
Global dimX.f, dimZ.i, ground.i
Global planeMesh.i,cubeMesh.i, wheelMesh.i
Global i.i, cube1.i,cube2.i
EnableExplicit
;************************************************************************************
;- ---- Procedures ----
;************************************************************************************
; Envoie au segment N l'ordre d'atteindre l'angle voulu.
Procedure enterCommand(numSegment.i, targetAngle.f)
; Si on est déjà à l'angle cible, ou si ce dernier dépasse les limites, on sort.
If targetAngle = roboPart(numSegment)\targetAngle
ProcedureReturn
EndIf
If targetAngle < roboPart(numSegment)\minRotation Or targetAngle > roboPart(numSegment)\maxRotation
ProcedureReturn
EndIf
; Positionne la nouvelle cible
roboPart(numSegment)\targetAngle = targetAngle
; Positionne les bornes du mouvements (pas très utile)
If roboPart(numSegment)\oldAngle < roboPart(numSegment)\targetAngle
SetJointAttribute(roboPart(numSegment)\motor,#PB_HingeJoint_LowerLimit,roboPart(numSegment)\oldAngle )
SetJointAttribute(roboPart(numSegment)\motor,#PB_HingeJoint_UpperLimit,roboPart(numSegment)\targetAngle )
Else
SetJointAttribute(roboPart(numSegment)\motor,#PB_HingeJoint_LowerLimit,roboPart(numSegment)\targetAngle )
SetJointAttribute(roboPart(numSegment)\motor,#PB_HingeJoint_UpperLimit,roboPart(numSegment)\oldAngle )
EndIf
; Stocke quand le mouvement commence et combien de temps il va prendre.
roboPart(numSegment)\startMvtTimer = ElapsedMilliseconds()
roboPart(numSegment)\timeBeforeStop = Abs(roboPart(numSegment)\targetAngle - roboPart(numSegment)\oldAngle) / roboPart(numSegment)\motorSpeed
; Dit au moteur concerné: "mets-toi en marche de façon à atteindre l'angle indiqué en N secondes".
; (Attention: le moteur se met en marche, mais ne s'arrête pas une fois l'angle atteint!)
HingeJointMotorTarget(roboPart(numSegment)\motor,roboPart(numSegment)\targetAngle,roboPart(numSegment)\timeBeforeStop)
; On colore le segment en rouge, pour que l'on voit que le moteur fonctionne.
SetEntityMaterial(roboPart(numSegment)\segment,MaterialID(mtRed))
EndProcedure
; Surveille quand un moteur doit s'arrêter.
Procedure checkForStop(numSegment.i)
Protected increment.f
; Si le moteur est en mouvement:
If roboPart(numSegment)\timeBeforeStop > 0
; On positionne des bornes du mouvement sur l'angle courant théorique (pour rigidifier le système)
increment = (ElapsedMilliseconds() - roboPart(numSegment)\startMvtTimer)/(roboPart(numSegment)\timeBeforeStop * 1000)
SetJointAttribute(roboPart(numSegment)\motor,#PB_HingeJoint_LowerLimit,roboPart(numSegment)\oldAngle + (roboPart(numSegment)\targetAngle-roboPart(numSegment)\oldAngle)*increment )
SetJointAttribute(roboPart(numSegment)\motor,#PB_HingeJoint_UpperLimit,roboPart(numSegment)\oldAngle + (roboPart(numSegment)\targetAngle-roboPart(numSegment)\oldAngle)*increment )
; Si le moteur doit s'arrêter:
If ElapsedMilliseconds() - roboPart(numSegment)\startMvtTimer >= roboPart(numSegment)\timeBeforeStop * 1000
; On stocke la valeur de l'angle cible, et on signale que le moteur est arrêté.
roboPart(numSegment)\oldAngle = roboPart(numSegment)\targetAngle
roboPart(numSegment)\timeBeforeStop = 0
; On le colore en bleu pour dire qu'il est au repos
SetEntityMaterial(roboPart(numSegment)\segment,MaterialID(mtBlue))
EndIf
Else
; Si le moteur est censé être arrêté, on fait tout ce que l'on peut pour le bloquer dans la position actuelle.
; (mais ça ne marche pas...)
SetJointAttribute(roboPart(numSegment)\motor,#PB_HingeJoint_LowerLimit,roboPart(numSegment)\targetAngle)
SetJointAttribute(roboPart(numSegment)\motor,#PB_HingeJoint_UpperLimit,roboPart(numSegment)\targetAngle)
HingeJointMotorTarget(roboPart(numSegment)\motor,roboPart(numSegment)\targetAngle,0.1)
EndIf
EndProcedure
; Créer la fenêtre et les champs de saisie
Global WMain
Global Frame3D_0, BT_enter, label_base, FL_base, label_arm1, FL_arm1, label_arm2, FL_arm2, label_arm3, FL_arm3, label_arm4, FL_arm4, label_finger, FL_finger
Procedure OpenWMain()
WMain = OpenWindow(#PB_Any, 0, 0, #SCREENWIDTH, #SCREENHEIGHT, "Bras robot", #PB_Window_SystemMenu)
Frame3D_0 = Frame3DGadget(#PB_Any, 1, 0, 298, 495, "Parameters")
BT_enter = ButtonGadget(#PB_Any, 90, 460, 100, 25, "Enter command")
label_base = TextGadget(#PB_Any, 20, 43, 70, 20, "Base:", #PB_Text_Right)
FL_base = StringGadget(#PB_Any, 90, 40, 80, 20, "90")
GadgetToolTip(FL_base, "Rotation de la base")
label_arm1 = TextGadget(#PB_Any, 20, 73, 70, 20, "Segment 1:", #PB_Text_Right)
FL_arm1 = StringGadget(#PB_Any, 90, 70, 80, 20, "45")
GadgetToolTip(FL_arm1, "Angle du premier segment du bras")
label_arm2 = TextGadget(#PB_Any, 20, 103, 70, 20, "Segment 2:", #PB_Text_Right)
FL_arm2 = StringGadget(#PB_Any, 90, 100, 80, 20, "110")
GadgetToolTip(FL_arm2, "Angle du second segment du bras.")
label_arm3 = TextGadget(#PB_Any, 20, 133, 70, 20, "Segment 3:", #PB_Text_Right)
FL_arm3 = StringGadget(#PB_Any, 90, 130, 80, 20, "-55")
GadgetToolTip(FL_arm3, "Angle du troisième segment du bras.")
label_arm4 = TextGadget(#PB_Any, 20, 163, 70, 20, "Segment 4:", #PB_Text_Right)
FL_arm4 = StringGadget(#PB_Any, 90, 160, 80, 20, "0")
GadgetToolTip(FL_arm4, "Rotation de quatrième segment du bras.")
label_finger = TextGadget(#PB_Any, 5, 193, 85, 20, "Fermeture doigts:", #PB_Text_Right)
FL_finger = StringGadget(#PB_Any, 90, 190, 80, 20, "-45")
GadgetToolTip(FL_finger, "angle de fermeture des doigts")
EndProcedure
; Gérer les évènements (le bouton "enter command", surtout)
Procedure Wmain_Events(event)
Select event
Case #PB_Event_CloseWindow
ProcedureReturn #False
Case #PB_Event_Menu
Select EventMenu()
EndSelect
Case #PB_Event_Gadget
Select EventGadget()
Case BT_enter
enterCommand(#BASE,ValF(GetGadgetText(FL_base)) )
enterCommand(#ARM1,ValF(GetGadgetText(FL_arm1)) )
enterCommand(#ARM2,ValF(GetGadgetText(FL_arm2)) )
enterCommand(#ARM3,ValF(GetGadgetText(FL_arm3)) )
enterCommand(#ARM4,ValF(GetGadgetText(FL_arm4)) )
enterCommand(#FINGER1,ValF(GetGadgetText(FL_finger)) )
enterCommand(#FINGER2,ValF(GetGadgetText(FL_finger)) )
EndSelect
EndSelect
ProcedureReturn #True
EndProcedure
;************************************************************************************
;- ---- Main program ----
;************************************************************************************
;- Initialization
InitEngine3D()
InitSprite()
InitKeyboard()
;- Window
OpenWmain()
OpenWindowedScreen(WindowID(Wmain), 300, 0, #SCREENWIDTH-300,#SCREENHEIGHT, 0, 0, 0,#PB_Screen_SmartSynchronization)
;-Texture
txBlank = CreateTexture(#PB_Any,4,4)
StartDrawing(TextureOutput(txBlank))
Box(0, 0, 4,4, $FFFFFF)
StopDrawing()
txGround = CreateTexture(#PB_Any,256,256)
StartDrawing(TextureOutput(txGround))
Box(0, 0, 256,256, $007700)
DrawingMode(#PB_2DDrawing_Outlined)
Box(0, 0, 256,256, $00BB00)
StopDrawing()
;-Materials
mtGround= CreateMaterial(#PB_Any,TextureID(txGround))
mtRed = CreateMaterial(#PB_Any,TextureID(txBlank))
SetMaterialColor(mtRed,#PB_Material_AmbientColor,$0000FF)
mtBlue = CreateMaterial(#PB_Any,TextureID(txBlank))
SetMaterialColor(mtBlue,#PB_Material_AmbientColor,$FF0000)
;- Terrain
dimX = 50:dimZ = 50
planeMesh = CreatePlane(#PB_Any,dimX,dimZ,1,1,dimX,dimZ)
ground = CreateEntity(#PB_Any, MeshID(planeMesh), MaterialID(mtGround))
EntityPhysicBody(ground, #PB_Entity_StaticBody,0,0,1)
;- Robot
cubeMesh = CreateCube(#PB_Any,1)
wheelMesh = CreateCylinder(#PB_Any,1,1)
; Base
roboPart(#BASE)\segment = CreateEntity(#PB_Any,MeshID(wheelMesh),MaterialID(mtBlue),0,0.1,0)
ScaleEntity(roboPart(#BASE)\segment,0.5,0.2,0.5)
EntityPhysicBody(roboPart(#BASE)\segment,#PB_Entity_CylinderBody,1,0,0)
roboPart(#BASE)\motor = HingeJoint(#PB_Any,EntityID(roboPart(#BASE)\segment),0,-0.1,0,0,-1,0,EntityID(ground),0,0,0,0,-1,0)
roboPart(#BASE)\minRotation = -170
roboPart(#BASE)\maxRotation = 170
roboPart(#BASE)\motorSpeed = 10
EnableHingeJointAngularMotor(roboPart(#BASE)\motor,#True,1,1)
; segment 1
roboPart(#ARM1)\segment = CreateEntity(#PB_Any,MeshID(cubeMesh),MaterialID(mtBlue),0,0.9,0)
ScaleEntity(roboPart(#ARM1)\segment,0.2,1,0.2)
EntityPhysicBody(roboPart(#ARM1)\segment,#PB_Entity_BoxBody,0.1,0,0)
roboPart(#ARM1)\motor = HingeJoint(#PB_Any,EntityID(roboPart(#ARM1)\segment),0,-0.6,0,1,0,0,EntityID(roboPart(#BASE)\segment),0,0.2,0,1,0,0)
roboPart(#ARM1)\minRotation = -70
roboPart(#ARM1)\maxRotation = 70
roboPart(#ARM1)\motorSpeed = 20
EnableHingeJointAngularMotor(roboPart(#ARM1)\motor,#True,1,10)
; segment 2
roboPart(#ARM2)\segment = CreateEntity(#PB_Any,MeshID(cubeMesh),MaterialID(mtBlue),0,2.2,0)
ScaleEntity(roboPart(#ARM2)\segment,0.2,0.8,0.2)
EntityPhysicBody(roboPart(#ARM2)\segment,#PB_Entity_BoxBody,0.05,0,0)
roboPart(#ARM2)\motor= HingeJoint(#PB_Any,EntityID(roboPart(#ARM2)\segment),0,-0.55,0,1,0,0,EntityID(roboPart(#ARM1)\segment),0,0.5,0,1,0,0)
roboPart(#ARM2)\minRotation = -110
roboPart(#ARM2)\maxRotation = 110
roboPart(#ARM2)\motorSpeed = 20
EnableHingeJointAngularMotor(roboPart(#ARM2)\motor,#True,1,10)
; segment 3
roboPart(#ARM3)\segment = CreateEntity(#PB_Any,MeshID(cubeMesh),MaterialID(mtBlue),0,2.7,0)
ScaleEntity(roboPart(#ARM3)\segment,0.2,0.2,0.2)
EntityPhysicBody(roboPart(#ARM3)\segment,#PB_Entity_BoxBody,0.05,0,0)
roboPart(#ARM3)\motor= HingeJoint(#PB_Any,EntityID(roboPart(#ARM3)\segment),0,-0.25,0,1,0,0,EntityID(roboPart(#ARM2)\segment),0,0.4,0,1,0,0)
roboPart(#ARM3)\minRotation = -110
roboPart(#ARM3)\maxRotation = 110
roboPart(#ARM3)\motorSpeed = 20
EnableHingeJointAngularMotor(roboPart(#ARM3)\motor,#True,1,10)
; segment 4 (cylindre)
roboPart(#ARM4)\segment = CreateEntity(#PB_Any,MeshID(wheelMesh),MaterialID(mtBlue),0,2.9,0)
ScaleEntity(roboPart(#ARM4)\segment,0.1,0.1,0.1)
EntityPhysicBody(roboPart(#ARM4)\segment,#PB_Entity_CylinderBody,0.05,0,0)
roboPart(#ARM4)\motor = HingeJoint(#PB_Any,EntityID(roboPart(#ARM4)\segment),0,-0.1,0,0,-1,0,EntityID(roboPart(#ARM3)\segment),0,0.1,0,0,-1,0)
roboPart(#ARM4)\minRotation = -170
roboPart(#ARM4)\maxRotation = 170
roboPart(#ARM4)\motorSpeed = 30
EnableHingeJointAngularMotor(roboPart(#ARM4)\motor,#True,1,1)
; Finger 1
roboPart(#FINGER1)\segment = CreateEntity(#PB_Any,MeshID(cubeMesh),MaterialID(mtBlue),-0.2,3.1,0)
ScaleEntity(roboPart(#FINGER1)\segment,0.05,0.2,0.1)
EntityPhysicBody(roboPart(#FINGER1)\segment,#PB_Entity_BoxBody,0.05,0,1)
roboPart(#FINGER1)\motor = HingeJoint(#PB_Any,EntityID(roboPart(#FINGER1)\segment),0,-0.3,0,0,0,-1,EntityID(roboPart(#ARM4)\segment),-0.1,-0.1,0,0,0,-1)
roboPart(#FINGER1)\minRotation = -45
roboPart(#FINGER1)\maxRotation = 20
roboPart(#FINGER1)\motorSpeed = 20
EnableHingeJointAngularMotor(roboPart(#FINGER1)\motor,#True,1,1)
; Finger 2
roboPart(#FINGER2)\segment = CreateEntity(#PB_Any,MeshID(cubeMesh),MaterialID(mtBlue),0.2,3.1,0)
ScaleEntity(roboPart(#FINGER2)\segment,0.05,0.2,0.1)
EntityPhysicBody(roboPart(#FINGER2)\segment,#PB_Entity_BoxBody,0.05,0,1)
roboPart(#FINGER2)\motor = HingeJoint(#PB_Any,EntityID(roboPart(#FINGER2)\segment),0,-0.3,0,0,0,1,EntityID(roboPart(#ARM4)\segment),0.1,-0.1,0,0,0,1)
roboPart(#FINGER2)\minRotation = -45
roboPart(#FINGER2)\maxRotation = 20
roboPart(#FINGER2)\motorSpeed = 20
EnableHingeJointAngularMotor(roboPart(#FINGER2)\motor,#True,1,1)
; Objets à prendre
cube1 = CreateEntity(#PB_Any,MeshID(cubeMesh),#PB_Material_None,-2,0.1,0)
ScaleEntity(cube1,0.2,0.2,0.2)
EntityPhysicBody(cube1,#PB_Entity_BoxBody,0.05,0,1)
cube2 = CreateEntity(#PB_Any,MeshID(cubeMesh),#PB_Material_None,-2,0.3,0)
ScaleEntity(cube2,0.2,0.2,0.2)
RotateEntity(cube2,0,30,0)
EntityPhysicBody(cube2,#PB_Entity_BoxBody,0.05,0,1)
; Faible gravité, car les joints/moteurs ne sont pas assez rigides pour tenir leur position
WorldGravity(-0.2)
;- Camera
CreateCamera(0, 0, 0, 100, 100)
MoveCamera(0,0,3,5,#PB_Absolute)
;-Light
AmbientColor($333333)
CreateLight(0,$FFFFFF,200,400,200)
WorldShadows(#PB_Shadow_Additive)
;- Main loop
KeyboardMode(#PB_Keyboard_International)
Repeat
Delay(1)
; Windows events (reset button)
eventID = WindowEvent()
While eventID <> 0 And notQuit = #True
notQuit = Wmain_Events(eventId)
eventID = WindowEvent()
Wend
ExamineKeyboard()
; Activate wireframe render
If KeyboardReleased(#PB_Key_W)
wireframe = 1-wireframe
If wireframe = #True
CameraRenderMode(0,#PB_Camera_Wireframe)
Else
CameraRenderMode(0,#PB_Camera_Textured)
EndIf
EndIf
; Activate physics render
If KeyboardReleased(#PB_Key_P)
viewBodies = 1-viewBodies
If viewBodies = #True
WorldDebug(#PB_World_DebugBody)
Else
WorldDebug(#PB_World_DebugNone)
EndIf
EndIf
; Start/pause simulation
If KeyboardReleased(#PB_Key_Return)
simRunning = 1-simRunning
EnableWorldPhysics(simRunning)
EndIf
; Activer / désactiver les moteurs
For i = 0 To #END_SEGMENTLIST - 1
DisableEntityBody(roboPart(i)\segment,#False)
checkForStop(i)
Next i
; Garder les autres objets "en éveil"
DisableEntityBody(cube1,#False)
DisableEntityBody(cube2,#False)
;- Camera management
CameraLookAt(0,0,2,2.5)
; Render
RenderWorld()
FlipBuffers()
Until KeyboardPushed(#PB_Key_Escape) Or notQuit = #False
End
)