PureBasic

Forums PureBasic - Français
https://www.purebasic.fr/french/

Bras robot

https://www.purebasic.fr/french/viewtopic.php?t=13671

Page 1 sur 1

Bras robot

Publié : ven. 24/mai/2013 9:19
par kelebrindae
Salut à tous,

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 ! :mrgreen:

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

Re: Bras robot

Publié : sam. 25/mai/2013 13:42
par graph100
@Kelebrindae : merci d'avoir prété l'oreille à mes idées tordues :lol:

Cette histoire de joint souple est vraiment embêtant.

J'ai re-regardé les instructions disponibles sur pb, et si il est possible d'appliquer une force ou une impulsion sur une entity, y appliquer un couple ne l'est pas.
l'instruction ingemotor le fait peut etre, mais ce n'est pas clair.
Fuseau horaire sur UTC+01:00
Page 1 sur 1

Développé par phpBB® Forum Software © phpBB Limited

Traduction française officielle © Qiaeru