-- Descrizione macchina Saomad-KAIROS by EgalWare s.r.l.   2024/07/08
-- 2024/06/27 ver 2.6f1 Prima versione (derivata da Kairos023)
-- 2024/07/01 ver 2.6g1 Aggiornato 3D con nuovo gruppo testa
-- 2024/07/03 ver 2.6g2 Aggiornato 3D con modello reale e altre piccole modifiche
-- 2024/07/08 ver 2.6g3 Aggiunta gestione sega a catena (MANCA generazione codice CN).

require( 'EmtGenerator')
EgtEnableDebug( false)

PP_VER = '2.6g3'
MIN_MACH_VER = '2.5j1'

EgtOutLog ( '** Saomad-KAIROS '..PP_VER..' (MinMach '.. MIN_MACH_VER ..') **', 1)

-- Parametri macchina
NumericalControl = 'SIEMENS'
ChainSaw = false
MinY = -823
MaxY = 520
ParkY = 400
FmaxY = 45000
MinZ = -298
MaxZ = 915
ParkZ = 900
FmaxZ = 45000
MinA = -120
MaxA = 120
ParkA = 0
FmaxA = 10440
MinC = -240
MaxC = 240
ParkC = -90
FmaxC = 18000
MillOffs = 211.0
ChSawLen = 141.5
MinX1 = 62.0
MaxX1 = 3650.0
ParkX1 = 480.0
FmaxX1 = 45000
MinX2 = -3650.0
MaxX2 = -62.0
ParkX2 = -480.0
FmaxX2 = 45000
LoadT = 1800.0
UnloadT = -1800
UnloadSmT = UnloadT
TurnerOffs = 0
MaxHOpen = 301
MaxVOpen = 621
LenToPress = 500
DeltaTabY = 0
DeltaTabZ = 0
DimTabY = 500
DimTabX = 34000
DistBack = 880
StartRotation = true
AutomaticRotation = true
AutoRotMinLen = 2600
MaxUnloadLen = 0
DefTcPos = 'T1'
CoeffVM = 0.5

-- Aggiornamento con dati da B&W
local sData = EgtGetSourceDir().."Beam\\EbwData.lua"
if EgtExistsFile( sData) then
   local Machine = dofile( sData)
   if Machine then
      if Machine.Offsets then
         if Machine.Offsets.TIPO_CN == 0 then
            NumericalControl =  'SIEMENS'
         end
         if Machine.Offsets.MIN_Y then MinY = Machine.Offsets.MIN_Y end
         if Machine.Offsets.MAX_Y then MaxY = Machine.Offsets.MAX_Y end
         if Machine.Offsets.MIN_Z then MinZ = Machine.Offsets.MIN_Z end
         if Machine.Offsets.MAX_Z then MaxZ = Machine.Offsets.MAX_Z end
         if Machine.Offsets.MIN_A then MinA = Machine.Offsets.MIN_A end
         if Machine.Offsets.MAX_A then MaxA = Machine.Offsets.MAX_A end
         if Machine.Offsets.MIN_C then MinC = Machine.Offsets.MIN_C end
         if Machine.Offsets.MAX_C then MaxC = Machine.Offsets.MAX_C end
         if Machine.Offsets.MILL_PIVOT then MillOffs = - Machine.Offsets.MILL_PIVOT end
         if Machine.Offsets.MIN_X1 then MinX1 = Machine.Offsets.MIN_X1 end
         if Machine.Offsets.MAX_X1 then MaxX1 = Machine.Offsets.MAX_X1 end
         if Machine.Offsets.MIN_X2 then MinX2 = Machine.Offsets.MIN_X2 end
         if Machine.Offsets.MAX_X2 then MaxX2 = Machine.Offsets.MAX_X2 end
         ParkX1 = Machine.Offsets.PARK_X1 or ParkX1
         ParkX2 = Machine.Offsets.PARK_X2 or ParkX2
         if Machine.Offsets.BEAM_LOAD then LoadT = Machine.Offsets.BEAM_LOAD end
         if Machine.Offsets.BEAM_UNLOAD then UnloadT = Machine.Offsets.BEAM_UNLOAD end
         TurnerOffs = Machine.Offsets.TURN_OFFS or TurnerOffs
         if Machine.Offsets.TAB_OFFSET_Y then DeltaTabY = Machine.Offsets.TAB_OFFSET_Y end
         if Machine.Offsets.TAB_OFFSET_Z then DeltaTabZ = Machine.Offsets.TAB_OFFSET_Z end
         if Machine.Offsets.NOULOAD then MaxUnloadLen = Machine.Offsets.NOULOAD end
      end
      ParkY = EgtClamp( ParkY, MinY, MaxY)
      ParkZ = EgtClamp( ParkZ, MinZ, MaxZ)
      ParkA = EgtClamp( ParkA, MinA, MaxA)
      ParkC = EgtClamp( ParkC, MinC, MaxC)
      UnloadSmT = UnloadT
   end
end

EmtGeneral {
      File='Saomad-KAIROS.nge',
      Offset = Vector3d( 0.0, -835, -779.06),
      AxisMaxAdjust = 30,
      AxisMaxRotAdj = 0.5,
      ExitMaxAdjust = 30,
      ExitMaxRotAdj = 0.5,
      AngDeltaMinForHome = 165,
      Special = 'Saomad-KAIROS.mlse',
      Processor = 'Saomad-KAIROS.mlpe'}
local BaseId = EmtBase {
      Name = 'Base',
      Geo='BASE/GEO',
      Aux={'BASE/SOLID', 'BASE/LOAD', 'BASE/UNLOAD', 'BASE/TC', 'BASE/TC2', 'BASE/SIGN', 'BASE/COLLISION'}}
local YId = EmtAxis {
      Name = 'Y',
      Parent = 'Base',
      Token = 'Y1',
      Type = MCH_AT.LINEAR,
      Dir = Y_AX(),
      Pos = Point3d( -278.7237, 322.3254, 2068.04),
      Stroke = {MinY, MaxY},
      Home = ParkY,
      Geo = 'Y_AXIS/GEO',
      Aux = 'Y_AXIS/SOLID'}
EmtAxis {
      Name = 'Z',
      Parent = 'Y',
      Token = 'Z1',
      Type = MCH_AT.LINEAR,
      Dir = Z_AX(),
      Pos = Point3d( 0.0257, -151.1589, 627.0044),
      Stroke = {MinZ, MaxZ},
      Home = ParkZ,
      Geo = 'Z_AXIS/GEO',
      Aux = {'Z_AXIS/SOLID', 'Z_AXIS/COLLISION'}}
local CId = EmtAxis {
      Name = 'C',
      Parent = 'Z',
      Token = 'C1',
      Type = MCH_AT.ROTARY,
      Dir = -Z_AX(),
      Pos = Point3d( 0, 0, 600.0),
      Stroke = {MinC, MaxC},
      Home = ParkC,
      Geo = 'C_AXIS/GEO',
      Aux = {'C_AXIS/SOLID', 'C_AXIS/COLLISION'}}
EmtAxis {
      Name = 'A',
      Parent = 'C',
      Token = 'A1',
      Type = MCH_AT.ROTARY,
      Dir = X_AX(),
      Pos = Point3d( 0, 0, MillOffs),
      Stroke = {MinA, MaxA},
      Home = ParkA,
      Geo = 'A_AXIS/GEO',
      Aux = {'A_AXIS/SOLID', 'A_AXIS/COLLISION'}}
-- Frese
local H1Id = EmtHead {
      Name = 'H1',
      Parent = 'A',
      HSet = 'H1',
      Type = MCH_HT.STD,
      Pos = Point3d( 0, 0, 0),
      TDir = Z_AX(),
      ADir = X_AX(),
      Rot1W = 2,
      OthColl = {'A/SOLID', 'C/SOLID'},
      Geo = 'H1_HEAD/GEO'}
EgtSetInfo( H1Id, 'ZMAXONROT', '1,60')
-- Sega a catena
EmtAxis {
      Name = 'CS',
      Parent = 'A',
      Token = '**',
      Type = MCH_AT.ROTARY,
      Dir = Z_AX(),
      Pos = Point3d( 0, 0, 0),
      Stroke = { -90, 0},
      Home = 0,
      Geo = 'CS_AXIS/GEO'}
local H3Id = EmtHead {
      Name = 'H3',
      Parent = 'CS',
      HSet = 'H1',
      Type = MCH_HT.STD,
      Pos = Point3d( 0, 0, -ChSawLen),
      TDir = Y_AX(),
      ADir = Z_AX(),
      Rot1W = 0.2,
      SolCh = MCH_SCC.ADIR_NEAR,
      OthColl = {'A/SOLID', 'C/SOLID'},
      Geo = 'H3_HEAD/GEO'}
EgtSetInfo( H3Id, 'ZMAXONROT', '1,5')
-- Morse
local X1Id = EmtAxis {
      Name = 'X1',
      Token = 'X1',
      Parent = 'Base',
      Type = MCH_AT.LINEAR,
      Dir = -X_AX(),
      Pos = Point3d( -1193.5, 245.0, 918.94),
      Stroke = { MinX1, MaxX1},
      Home = ParkX1,
      Geo = 'X1_AXIS/GEO',
      Aux = {'X1_AXIS/SOLID', 'X1_AXIS/COLLISION'}}
local PX1Id = EmtAxis {
      Name = 'PX1',
      Parent = 'X1',
      Type = MCH_AT.LINEAR,
      Dir = -Y_AX(),
      Pos = Point3d( -1193.5, 95.0, 918.94),
      Stroke = { 0, MaxHOpen},
      Home = MaxHOpen,
      Geo = 'PX1_AXIS/GEO',
      Aux = {'PX1_AXIS/SOLID', 'PX1_AXIS/COLLISION'}}
local QX1Id = EmtAxis {
      Name = 'QX1',
      Parent = 'X1',
      Type = MCH_AT.LINEAR,
      Dir = Z_AX(),
      Pos = Point3d( -814.2, -22.5, 668.9787),
      Stroke = { 0, MaxVOpen},
      Home = MaxVOpen,
      Geo = 'QX1_AXIS/GEO',
      Aux = 'QX1_AXIS/SOLID'}
local X2Id = EmtAxis {
      Name = 'X2',
      Token = 'X2',
      Parent = 'Base',
      Type = MCH_AT.LINEAR,
      Dir = -X_AX(),
      Pos = Point3d( 1193.5001, 245.0, 918.94),
      Stroke = { MinX2, MaxX2},
      Home = ParkX2,
      Geo = 'X2_AXIS/GEO',
      Aux = {'X2_AXIS/SOLID', 'X2_AXIS/COLLISION'}}
local PX2Id = EmtAxis {
      Name = 'PX2',
      Parent = 'X2',
      Type = MCH_AT.LINEAR,
      Dir = -Y_AX(),
      Pos = Point3d( 1193.5001, 95.0, 918.94),
      Stroke = { 0, MaxHOpen},
      Home = MaxHOpen,
      Geo = 'PX2_AXIS/GEO',
      Aux = {'PX2_AXIS/SOLID', 'PX2_AXIS/COLLISION'}}
local QX2Id = EmtAxis {
      Name = 'QX2',
      Parent = 'X2',
      Type = MCH_AT.LINEAR,
      Dir = Z_AX(),
      Pos = Point3d( 814.2001, 640.4727, -1591.561),
      Stroke = { 0, MaxVOpen},
      Home = MaxVOpen,
      Geo = 'QX2_AXIS/GEO',
      Aux = 'QX2_AXIS/SOLID'}
EmtAxis {
      Name = 'T',
      Parent = 'Base',
      --Token = '**',
      Type = MCH_AT.LINEAR,
      Dir = - X_AX(),
      Pos = Point3d( 0, 0, 0),
      Stroke = { -30000, 30000},
      Home = LoadT,
      Geo = 'T_AXIS/GEO'}
EmtTable {
      Name = 'Tab',
      Parent = 'T',
      Type = MCH_TT.FLAT,
      Ref1 = Point3d( -DimTabX, DeltaTabY - DimTabY, DeltaTabZ),
      Scale = { DimTabX / 20000, DimTabY / 500, 1},
      Geo = 'TABLE/GEO',
      Aux = 'TABLE/SOLID'}
-- ToolChanger 10 posizioni verticale
local ptTcCen = Point3d( -520.9759, -817, 1056.0424)
EmtTcPos {
      Name = 'T101',
      Parent = 'Base',
      Pos = ptTcCen,
      TDir = X_AX(),
      ADir = Y_AX(),
      Geo = 'BASE/T101'}
EmtTcPos {
      Name = 'T102',
      Parent = 'Base',
      Pos = ptTcCen - Vector3d( 0, 0, 110),
      TDir = X_AX(),
      ADir = Y_AX(),
      Geo = 'BASE/T102'}
EmtTcPos {
      Name = 'T103',
      Parent = 'Base',
      Pos = ptTcCen - Vector3d( 0, 0, 110*2),
      TDir = X_AX(),
      ADir = Y_AX(),
      Geo = 'BASE/T103'}
EmtTcPos {
      Name = 'T104',
      Parent = 'Base',
      Pos = ptTcCen - Vector3d( 0, 0, 110*3),
      TDir = X_AX(),
      ADir = Y_AX(),
      Geo = 'BASE/T104'}
EmtTcPos {
      Name = 'T105',
      Parent = 'Base',
      Pos = ptTcCen - Vector3d( 0, 0, 110*4),
      TDir = X_AX(),
      ADir = Y_AX(),
      Geo = 'BASE/T105'}
EmtTcPos {
      Name = 'T106',
      Parent = 'Base',
      Pos = ptTcCen - Vector3d( 0, 0, 110*5),
      TDir = X_AX(),
      ADir = Y_AX(),
      Geo = 'BASE/T106'}
EmtTcPos {
      Name = 'T107',
      Parent = 'Base',
      Pos = ptTcCen - Vector3d( 0, 0, 110*6),
      TDir = X_AX(),
      ADir = Y_AX(),
      Geo = 'BASE/T107'}
EmtTcPos {
      Name = 'T108',
      Parent = 'Base',
      Pos = ptTcCen - Vector3d( 0, 0, 110*7),
      TDir = X_AX(),
      ADir = Y_AX(),
      Geo = 'BASE/T108'}
EmtTcPos {
      Name = 'T109',
      Parent = 'Base',
      Pos = ptTcCen - Vector3d( 0, 0, 110*8),
      TDir = X_AX(),
      ADir = Y_AX(),
      Geo = 'BASE/T109'}
EmtTcPos {
      Name = 'T110',
      Parent = 'Base',
      Pos = ptTcCen - Vector3d( 0, 0, 110*9),
      TDir = X_AX(),
      ADir = Y_AX(),
      Geo = 'BASE/T110'}
EmtTcPos {
      Name = 'T111',
      Parent = 'Base',
      Pos = ptTcCen - Vector3d( 0, 0, 110*10),
      TDir = X_AX(),
      ADir = Y_AX(),
      Geo = 'BASE/T111'}
-- ToolChanger lama
EmtTcPos {
      Name = 'T201',
      Parent = 'Base',
      Pos = Point3d( 508.0241, -748.997, 271.8273),
      TDir = -X_AX(),
      ADir = -Y_AX(),
      Geo = 'BASE/T201'}
-- ToolChanger motosega
EmtTcPos {
      Name = 'T202',
      Parent = 'Base',
      Pos = Point3d( 656.5865, -768.913, 715.2899),
      TDir = -X_AX(),
      ADir = Z_AX(),
      Geo = 'BASE/T202'}

-- Aggiusto posizioni geometriche
local vtMove = Vector3d( 0, ( DeltaTabY - 0), ( DeltaTabZ - 0))
local vtMoveY = Vector3d( 0, 0, ( DeltaTabZ - 0))
EgtMove( EgtGetFirstNameInGroup( BaseId, 'SOLID'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( BaseId, 'LOAD'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( BaseId, 'UNLOAD'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( BaseId, 'TC'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( BaseId, 'TC2'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( BaseId, 'SIGN'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( BaseId, 'COLLISION'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( X1Id, 'SOLID'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( X1Id, 'COLLISION'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( PX1Id, 'SOLID'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( PX1Id, 'COLLISION'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( QX1Id, 'SOLID'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( X2Id, 'SOLID'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( X2Id, 'COLLISION'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( PX2Id, 'SOLID'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( PX2Id, 'COLLISION'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( QX2Id, 'SOLID'), vtMove, GDB_RT.GLOB)
EgtMove( EgtGetFirstNameInGroup( YId, 'SOLID'), vtMoveY, GDB_RT.GLOB)

-- Assegno identificativi alle spie delle morse
PX1LightId = EgtGetFirstNameInGroup( EgtGetFirstNameInGroup( PX1Id, 'SOLID') or GDB_ID.NULL, 'Light')
PX2LightId = EgtGetFirstNameInGroup( EgtGetFirstNameInGroup( PX2Id, 'SOLID') or GDB_ID.NULL, 'Light')

---------------------------------------------------------------------
-- Funzioni richiamate per modificare i dati macchina in casi particolari
--function OnSetTable()
--end

---------------------------------------------------------------------
function OnSetHead()
   -- Se testa con sega a catena
   if EMC.HEAD == 'H3' then
      -- aggiustamenti per distanza
      local dDist = EgtIf( EMC.DIST and abs( EMC.DIST) > 1, EMC.DIST, ChSawLen)
      EmtModifyExitPosition( EMC.HEAD, EMC.EXIT, Point3d( 0, 0, -dDist))
      -- recupero valore asse CS bloccato
      local sVal = EgtGetMachiningParam( MCH_MP.BLOCKEDAXIS)
      local dPosCS = tonumber( sVal:sub( 4) or '')
      EmtModifyAxisHome( 'C', GetChainSawCHomeFromVirtualAxis( dPosCS))
      -- limiti asse A
      EmtModifyAxisStroke( 'A', { -91, 91})
   -- altrimenti
   else
      EmtModifyAxisHome( 'C', ParkC)
      -- se lama
      if EMC.TCPOS == 'T201' then
         EmtModifyAxisStroke( 'A', { -100, 100})
      -- altrimenti
      else
         EmtModifyAxisStroke( 'A', { MinA, MaxA})
      end
   end
end

---------------------------------------------------------------------
-- Funzione che calcola l'angolo Home di C per la sega a catena dal valore dell'asse virtuale
function GetChainSawCHomeFromVirtualAxis( dPosCS)
   -- se CS=0 -> HomeC = -90
   if abs( dPosCS) < 0.1 then
      return -90
   -- altrimenti CS=-90 -> HomeC = -180
   else
      return -180
   end
end

---------------------------------------------------------------------
-- Funzione per impostare spia stato morsa carrello X1
function SetPX1Light( bClosed)
   if not PX1LightId then return end
   EgtSetColor( PX1LightId, EgtIf( bClosed, 'RED', 'LIME')) 
   if bClosed then
      EgtSetInfo( PX1LightId, 'On', '1')
   else
      EgtRemoveInfo( PX1LightId, 'On')
   end
end

---------------------------------------------------------------------
-- Funzione per leggere lo stato della morsa carrello X1
function GetPX1Light()
   if not PX1LightId then return false end
   return ( EgtGetInfo( PX1LightId, 'On') == '1')
end

---------------------------------------------------------------------
-- Funzione per impostare spia stato morsa carrello X2
function SetPX2Light( bClosed)
   if not PX2LightId then return end
   EgtSetColor( PX2LightId, EgtIf( bClosed, 'RED', 'LIME')) 
   if bClosed then
      EgtSetInfo( PX2LightId, 'On', '1')
   else
      EgtRemoveInfo( PX2LightId, 'On')
   end
end

---------------------------------------------------------------------
-- Funzione per leggere lo stato della morsa carrello X2
function GetPX2Light()
   if not PX2LightId then return false end
   return ( EgtGetInfo( PX2LightId, 'On') == '1')
end

---------------------------------------------------------------------
-- Funzione per resettare tutte le attivazioni della macchina
function OnResetMachine()
   EmtUnlinkAllRawPartsFromGroups()
   EmtUnlinkAllFixturesFromGroups()
   SetPX1Light( false)
   SetPX2Light( false)
   -- nascondo Vmill
   local nRawId = EgtGetFirstRawPart()
   while nRawId do
      local nVmId = EgtGetFirstNameInGroup( nRawId, 'VMill')
      local nId = EgtGetFirstInGroup( nRawId)
      while nId do
         EgtSetStatus( nId, EgtIf( nId ~= nVmId, GDB_ST.ON, GDB_ST.OFF))
         nId = EgtGetNext( nId)
      end
      nRawId = EgtGetNextRawPart( nRawId)
   end
   EgtSetStatus( EgtGetFirstNameInGroup( GDB_ID.ROOT, 'VMill') or GDB_ID.NULL, GDB_ST.OFF)
end