'V1 du 11/04/2017

'V2 du 29/11/2023
'IHM: Kinco GL070E sous DTools V4.0.0.0 

' Affectation des entrees
const TYPE_FDC                          = 1  ' type de contact fin de course 1 NO ou 0 NF
const IN_FDC_ORIGINE                    = 1  ' fin de course prise d origine



' axes
const AXE                               = 1  ' Clk1 et Dir1, Axe X

' parametres 
' entre parentheses adresse des registres accessibles par l IHM en 4X

const EE_RESOLUTION                     = 0  ' (5120) pulses/mm
const EE_ACCEL_HOM                      = 1  ' (5122) khz, acceleration pour prise d origine
const EE_ACCEL_MVT                      = 2  ' (5124) khz
const EE_VITESSE_MIN                    = 3  ' (5126) mm/s, vitesse minimale
const EE_VITESSE_MAX                    = 4  ' (5128) mm/s, vitesse maximale
const EE_VITESSE_HOME_R                 = 5  ' (5130) mm/s, vitesse rapide pour prise d origine
const EE_VITESSE_HOME_L                 = 6  ' (5132) mm/s, vitesse lente pour prise d origine
const EE_LONGUEUR_MIN                   = 7  ' (5134) mm longueur min de l axe
const EE_LONGUEUR_MAX                   = 8  ' (5136) mm longueur max de l axe
const EE_VITESSE_MVT                    = 9  ' (5138) mm/s
const EE_LARGEUR_COUPE                  = 10 ' (5140) mm
const EE_REF_ORIGINE                    = 11 ' (5142) mm
const EE_RETRAIT                        = 12 ' (5144) mm
const EE_POSITION                       = 13 ' (5146) mm
'5 positions: 5148 à 5157 -> EEData32 14 a 18

' bits modbus
const MBB_GHOME                         = 320  ' Cycle homing
const MBB_HOME_OK                       = 321  ' homing ok
const MBB_MODE_AUTO                     = 322  ' mode auto en cours

const MBB_MOVE                          = 328  ' pour aller a position
const MBB_MEMOIRE                       = 329  ' mise position en memoire

const MBB_RETRAIT                       = 330  ' pour reculer du retrait

const MBB_XX_PLUS                       = 336  ' deplacement manuel rapide
const MBB_XX_MOINS                      = 337  
const MBB_X_PLUS                        = 338  ' deplacement manuel rapide
const MBB_X_MOINS                       = 339


Resolution= GetEEData32(EE_RESOLUTION) / 100
DIM Position(5) ' tableau des positions


Initialisation:
  ? "Initialisation"
  Unlock

  GHome      = 0
  OUTALL 0 
  StopAxes &h1f

  for i=320 To 415 : SetMBBit i, 0 : Next i
  Pause 200
  SetPos AXE, 0
  
  Position(0) = 0
  for i= 1 to 5:
    Position(i) = (GetEEData32(EE_REF_ORIGINE) - GetEEData32(EE_POSITION+i)) /100 * Resolution
    '? Position(i)
  Next i
  CalculParametres()



DO

' L'entree ENABLE a ete coupee ou collision
  if stsbit(8)=1 then goto Initialisation

  if IsMBPrmChanged then CalculParametres()



' ***********************************************************************'
'                           Calculs pour Affichages 
' ***********************************************************************'

  SetUserMem 1, GetEEData32(EE_REF_ORIGINE) - (GetPos(AXE)/Resolution*100)

' ***********************************************************************'
'                              Deplacements manuels 
' ***********************************************************************'

    if  DFMBit(1, GetMBBit(MBB_XX_MOINS)) then
      ? "MoveAxe Plus ", AXE, ", ", Acceleration, ", ", Vitesse, ", ", Acceleration, ", ", PositionMax
      MoveAxe AXE, Acceleration, Vitesse, Acceleration, PositionMax
    endif

    if  DFMBit(2, GetMBBit(MBB_XX_PLUS)) then
      ? "MoveAxe Moins ", AXE, ", ", Acceleration, ", ", Vitesse, ", ", Acceleration, ", ", Position(0)
      MoveAxe AXE, Acceleration, Vitesse, Acceleration, Position(0)
    endif

    if  DFMBit(3, GetMBBit(MBB_X_MOINS)) then
      ? "MoveAxe Plus ", AXE, ", ", Acceleration, ", ", Vitesse/10, ", ", Acceleration, ", ", PositionMax
      MoveAxe AXE, Acceleration, Vitesse/10, Acceleration, PositionMax
    endif

    if  DFMBit(4, GetMBBit(MBB_X_PLUS)) then
      ? "MoveAxe Moins ", AXE, ", ", Acceleration, ", ", Vitesse/10, ", ", Acceleration, ", ", Position(0)
      MoveAxe AXE, Acceleration, Vitesse/10, Acceleration, Position(0)
    endif

    if  DFDBit(1, GetMBBit(MBB_XX_MOINS)) or DFDBit(2, GetMBBit(MBB_XX_PLUS)) or DFDBit(3, GetMBBit(MBB_X_MOINS)) or DFDBit(4, GetMBBit(MBB_X_PLUS)) then
      StopAxes &h1f
      ? "StopAxes &h1f "
    endif

' ***********************************************************************'
'                        Memorisation position
' ***********************************************************************'

  if not GetMBBit(MBB_MODE_AUTO) and GetMBBit(MBB_MEMOIRE) and GetMBBit(MBB_HOME_OK) then   
    SetEEData32 EE_POSITION, GetEEData32(EE_REF_ORIGINE) - (GetPos(AXE)/Resolution*100) ' Pour mise a jour du tableau des positions
    SetEEData32 EE_POSITION+GetUserMem(0), GetEEData32(EE_REF_ORIGINE) - (GetPos(AXE)/Resolution*100) ' Actualisation du registre sauvegardé correspondant

    CalculParametres()
    SetMBBit MBB_MEMOIRE, 0
  endif

' ***********************************************************************'
'                              Deplacements 
' ***********************************************************************'

  ' Deplacement a la position choisie (l'IHM écrit le numéro de la position dans le UserMem0 (Ram))
  if GetMBBit(MBB_MOVE) then 
    ? "MoveAxe ", AXE, ", ", Acceleration, ", ", Vitesse, ", ", Acceleration, ", ", Position(GetUserMem(0))
    MoveAxe AXE, Acceleration, Vitesse, Acceleration, Position(GetUserMem(0))
    SetMBBit MBB_MOVE, 0
  endif

  ' Mouvement de retrait
  if GetMBBit(MBB_RETRAIT) then 
    ? "MoveAxe ", AXE, ", ", Acceleration, ", ", Vitesse, ", ", Acceleration, ", ", GetPos(AXE)-(GetEEData32(EE_RETRAIT)/100*Resolution)
    MoveAxe AXE, Acceleration, Vitesse, Acceleration, GetPos(AXE)-(GetEEData32(EE_RETRAIT)/100*Resolution)
    SetMBBit MBB_RETRAIT, 0
  endif


' ***********************************************************************'
                            ' Cycle de Homing
' ***********************************************************************'


  if GHome = 0 then
    if GetMBBit(MBB_GHOME) then
      SetMBBit MBB_GHOME, 0
      GHome = 100
    endif
  elseif GHome = 100 then
  ' lancement de la procedure de homing
    ? "Lancement Homing"
    ? "  Home ", AXE, ", ", IN_FDC_ORIGINE, ", ", TYPE_FDC, ", ", Accel_Hom, ", ", Vit_Rapide_Hom, ", ", Accel_Hom, ", ", Course_Hom, ", ", Vit_Lente_Hom, ", ", 0
    Home AXE, IN_FDC_ORIGINE, TYPE_FDC,Accel_Hom,Vit_Rapide_Hom, Accel_Hom, Course_Hom, Vit_Lente_Hom,0
    GHome = 101
  elseif GHome = 101 then
    if not StsBit(50 + 2 * (AXE -1)) then        ' le homing est termine
      if not StsBit(51 + 2 * (AXE -1)) then      ' on verifie qu il ne s est pas termine en erreur
        SetMBBit MBB_HOME_OK, 1
        SetPos AXE, 0                            ' on remet a 0 la position de l axe
        ? "Origine axe : ", AXE, " SUCCES"
        SetMBBit MBB_HOME_OK, 1
        SetMBBit MBB_GHOME, 0
        GHome = 0
      else
        ? "Origine axe :  ", AXE, " ERREUR"
        SetMBBit MBB_HOME_OK, 0
        SetMBBit MBB_GHOME, 0
        GHome = 0
      endif
    endif
  endif


LOOP

sub CalculParametres()
  ? "Pre calcul axes"
' les nombres sont rentres sur 2 decimales mais stockes sous forme d entiers, il faut donc diviser par 100 chacunes des valeurs

  Resolution       = GetEEData32(EE_RESOLUTION) / 100

' calculs homing
  Accel_Hom        = GetEEData32(EE_ACCEL_HOM) / 100
  Vit_Rapide_Hom   = GetEEData32(EE_VITESSE_HOME_R) /100             * Resolution
  Vit_Lente_Hom    = GetEEData32(EE_VITESSE_HOME_L) /100             * Resolution
  'Course_Hom       = -1.1 * GetEEData32(EE_LONGUEUR_MAX) /100        * Resolution
  Course_Hom       = -11 * GetEEData32(EE_LONGUEUR_MAX) /100        * Resolution

' calculs deplacements
  Acceleration             = GetEEData32(EE_ACCEL_MVT) / 100
  Vitesse                  = GetEEData32(EE_VITESSE_MVT) / 100      * Resolution
  PositionMax              = GetEEData32(EE_REF_ORIGINE) /100 * Resolution

  Position(0)              = 0
  'Position(1)              = (GetEEData32(EE_REF_ORIGINE) - GetEEData32(EE_POSITION)) * Resolution / 100
  'Position(2)              = (GetEEData32(EE_REF_ORIGINE) - GetEEData32(EE_POSITION) - GetEEData32(EE_RETRAIT)) * Resolution / 100

  'Position de 1 à 5:
  Position(GetUserMem(0))  = (GetEEData32(EE_REF_ORIGINE) - GetEEData32(EE_POSITION)) /100 * Resolution

end sub




OnError:
  Outall 0
  SetAna 1,0
  SetAna 2,0
  StopAxes &h1F