andrea.villa
4605 lines
191 KiB
Plaintext
4605 lines
191 KiB
Plaintext
-- Special Operations macchina Essetre-FAST by EgalWare s.r.l. 2024/03/28
|
|
|
|
-- Intestazioni
|
|
require( 'EmtGenerator')
|
|
EgtEnableDebug( false)
|
|
|
|
-- carico librerie
|
|
local BD = require( 'BeamData')
|
|
|
|
---------------------------------------------------------------------
|
|
-- *** Generic Machinings ***
|
|
---------------------------------------------------------------------
|
|
pcall( require, 'EmtGenMachining') -- si fa una require con PCALL perchè la libreria è opzionale
|
|
|
|
|
|
---------------------- OnSpecialMoveZup -----------------------------
|
|
---------------------------------------------------------------------
|
|
function OnSpecialMoveZup()
|
|
--EgtOutLog( 'OnSpecialMoveZup : ' .. EMC.HEAD .. '.' .. tostring( EMC.EXIT))
|
|
|
|
-- Inizializzazioni
|
|
EMC.ERR = 0
|
|
EMC.MODIF = false
|
|
|
|
-- se sega a catena
|
|
if EMC.HEAD == 'H3' then
|
|
|
|
end
|
|
|
|
end
|
|
|
|
---------- OnSpecialApplyDisposition & OnPostApplyMachining ---------
|
|
----------------------- Costanti ------------------------------------
|
|
local DELTA_TOL_FIXED = 50
|
|
local DELTA_SIC = 1
|
|
local AGG_LOAD = 50
|
|
|
|
----------------------- Variabili -----------------------------------
|
|
local Test = false
|
|
|
|
---------------------------------------------------------------------
|
|
local function PrepareClGroup( nParentId)
|
|
|
|
local nClId = EgtGetFirstNameInGroup( nParentId, 'CL')
|
|
-- se non c'è, lo aggiunge
|
|
if not nClId then
|
|
nClId = EgtGroup( EMC.DISPID)
|
|
if not nClId then
|
|
return nil
|
|
end
|
|
EgtSetName( nClId, 'CL')
|
|
-- altrimenti lo svuoto
|
|
else
|
|
EgtEmptyGroup( nClId)
|
|
end
|
|
|
|
return nClId
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function IsStartOrRestPhase( nPhase)
|
|
local sVal = EgtGetInfo( EgtGetPhaseDisposition( nPhase) or GDB_ID.NULL, 'TYPE')
|
|
return ( sVal == 'START' or sVal == 'REST')
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function IsMidPhase( nPhase)
|
|
local sVal = EgtGetInfo( EgtGetPhaseDisposition( nPhase) or GDB_ID.NULL, 'TYPE')
|
|
return ( sVal == 'MID')
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function IsEndPhase( nPhase)
|
|
local sVal = EgtGetInfo( EgtGetPhaseDisposition( nPhase) or GDB_ID.NULL, 'TYPE')
|
|
return ( sVal == 'END')
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function IsMid2Phase( nPhase)
|
|
local sVal = EgtGetInfo( EgtGetPhaseDisposition( nPhase) or GDB_ID.NULL, 'TYPE')
|
|
return ( sVal == 'MID2')
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function IsEnd2Phase( nPhase)
|
|
local sVal = EgtGetInfo( EgtGetPhaseDisposition( nPhase) or GDB_ID.NULL, 'TYPE')
|
|
return ( sVal == 'END2')
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function GetNextStartOrRestPhase( nPhase)
|
|
local nNextPhase = nPhase + 1
|
|
while nNextPhase <= EgtGetPhaseCount() do
|
|
if IsStartOrRestPhase( nNextPhase) then
|
|
break ;
|
|
end
|
|
nNextPhase = nNextPhase + 1
|
|
end
|
|
return nNextPhase
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function GetPhaseRot( nPhase)
|
|
return ( EgtGetInfo( EgtGetPhaseDisposition( nPhase) or GDB_ID.NULL, 'ROT', 'i') or 0)
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function IsLastOperationBeforeRotation( nOperId)
|
|
-- se ultima fase o ultima fase del pezzo, ritorno risultato negativo
|
|
if EMC.PHASE == EgtGetPhaseCount() or IsEndPhase( EMC.PHASE) or IsEnd2Phase( EMC.PHASE) then
|
|
return false
|
|
end
|
|
-- recupero la successiva operazione attiva
|
|
local nNextOperId = EgtGetNextActiveOperation( nOperId)
|
|
-- se non esiste o non è una disposizione, ritorno risultato negativo
|
|
if not nNextOperId or EgtGetOperationType( nNextOperId) ~= MCH_OY.DISP then
|
|
return false
|
|
end
|
|
-- recupero le rotazioni della fase corrente e della prossima fase
|
|
local nRot = GetPhaseRot( EMC.PHASE)
|
|
local nNextRot = GetPhaseRot( EMC.PHASE + 1)
|
|
-- ritorno se sono diverse
|
|
return ( nRot ~= nNextRot)
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function IsFirstMachiningAfterRotation( nMchId)
|
|
-- se prima fase o prima fase del pezzo, ritorno risultato negativo
|
|
if EMC.PHASE == 1 or IsStartOrRestPhase( EMC.PHASE) then
|
|
return false
|
|
end
|
|
-- recupero la precedente operazione attiva
|
|
local nPrevOperId = EgtGetPrevActiveOperation( nMchId)
|
|
-- se non esiste o non è una disposizione, ritorno risultato negativo
|
|
if not nPrevOperId or EgtGetOperationType( nPrevOperId) ~= MCH_OY.DISP then
|
|
return false
|
|
end
|
|
-- recupero le rotazioni della fase corrente e della fase precedente
|
|
local nRot = GetPhaseRot( EMC.PHASE)
|
|
local nPrevRot = GetPhaseRot( EMC.PHASE - 1)
|
|
-- ritorno se sono diverse
|
|
return ( nRot ~= nPrevRot)
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function GetCUTID()
|
|
-- recupero CUTID del pezzo in lavoro
|
|
local nOrd = GetPhaseOrd( EMC.PHASE)
|
|
local nPartRawId
|
|
local nRawId = EgtGetFirstRawPart()
|
|
while nRawId do
|
|
local nRawOrd = EgtGetInfo( nRawId, 'ORD', 'i')
|
|
if nRawOrd == nOrd then
|
|
nPartRawId = nRawId
|
|
break
|
|
end
|
|
nRawId = EgtGetNextRawPart( nRawId)
|
|
end
|
|
local CutID = EgtGetInfo( EgtGetFirstPartInRawPart( nPartRawId or GDB_ID.NULL) or GDB_ID.NULL, 'CUTID', 'i') or 0
|
|
return CutID
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function EnsureZmax( bZmaxOk, vCmd)
|
|
if not bZmaxOk then table.insert( vCmd, { 1, 'Z', EgtGetAxisHomePos( 'Z')}) end
|
|
return true
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function EmitComment( vCmd, sOut)
|
|
EgtOutLog( ' ' .. sOut, 1)
|
|
if Test then
|
|
table.insert( vCmd, { 0, sOut})
|
|
end
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function OnSpecialApplyDisposition()
|
|
|
|
EgtOutLog( ' *** Fase : ' .. EgtNumToString( EMC.PHASE, 0) .. ' ***', 1)
|
|
|
|
-- Inizializzo codice di errore
|
|
EMC.ERR = 0
|
|
|
|
-- Campi obbligatori ma non usati
|
|
EMC.HEAD = ""
|
|
EMC.EXIT = 1
|
|
EMC.TCPOS = ""
|
|
EMC.SHIFTS = 0
|
|
EMC.SBH = false
|
|
|
|
-- Se disposizione da saltare non devo fare alcunché
|
|
if EgtExistsInfo( EMC.DISPID, 'SKIP') then return end
|
|
|
|
-- Assegno flag di pezzo separato dal resto del grezzo
|
|
local bSplit = IsEndPhase( EMC.PHASE) or IsMid2Phase( EMC.PHASE) or IsEnd2Phase( EMC.PHASE)
|
|
|
|
-- Recupero il tipo dell'operazione successiva
|
|
local nNextOpeType = EgtGetOperationType( EgtGetNextActiveOperation( EMC.DISPID) or GDB_ID.NULL)
|
|
|
|
-- Se ci sono lavorazioni successive non devo fare alcunché
|
|
if nNextOpeType ~= MCH_OY.NONE and nNextOpeType ~= MCH_OY.DISP then return end
|
|
|
|
-- Imposto gruppo e path di movimento
|
|
local nClId = PrepareClGroup( EMC.DISPID)
|
|
if not nClId then
|
|
EMC.ERR = 3
|
|
return
|
|
end
|
|
local nPathId = EgtGroup( nClId)
|
|
if not nPathId then
|
|
EMC.ERR = 6
|
|
return
|
|
end
|
|
EgtSetName( nPathId, 'Empty')
|
|
EMC.PATHID = nPathId
|
|
EMC.SHIFTS = -1
|
|
|
|
-- Se l'operazione successiva è una disposizione con rotazione, devo preparare il pezzo alla rotazione
|
|
if IsLastOperationBeforeRotation( EMC.DISPID) then
|
|
-- aggiornamento posizioni
|
|
if IsStartPhase( EMC.PHASE) then
|
|
-- carico le posizioni
|
|
local dPosT = EgtGetInfo( EMC.DISPID, 'TPOS', 'd')
|
|
local dPosY = EgtGetInfo( EMC.DISPID, 'YPOS', 'd')
|
|
EMC.TPOS = dPosT
|
|
EMC.YDELTA = dPosY - dPosT
|
|
EMC.VDELTA = nil
|
|
EMC.CNT = 1
|
|
else
|
|
local nPrevOpeId = EgtGetPrevActiveOperation( EMC.DISPID)
|
|
local nLastPathId = EgtGetLastInGroup( EgtGetFirstNameInGroup( nPrevOpeId, 'CL') or GDB_ID.NULL)
|
|
local nLastEntId = EgtGetLastInGroup( nLastPathId)
|
|
local vAxes = EmtGetAxesPos( nLastEntId)
|
|
if #vAxes > 0 then EMC.TPOS = vAxes[1] end
|
|
EMC.YDELTA = EgtGetInfo( nLastPathId, 'YDELTA', 'd')
|
|
EMC.VDELTA = EgtGetInfo( nLastPathId, 'VDELTA', 'd')
|
|
EMC.CNT = EgtGetInfo( nLastPathId, 'CNT', 'i')
|
|
end
|
|
-- Determinazione delle dimensioni del grezzo in lavoro
|
|
local b3Raw = BBox3d()
|
|
local nRawId = EgtGetFirstRawPart()
|
|
while nRawId do
|
|
if EgtVerifyRawPartPhase( nRawId, EMC.PHASE) then
|
|
b3Raw = EgtGetRawPartBBox( nRawId)
|
|
break
|
|
end
|
|
nRawId = EgtGetNextRawPart( nRawId)
|
|
end
|
|
EMC.LB = b3Raw:getDimX() + 10 * GEO.EPS_SMALL
|
|
-- Assegno sovramateriale di testa e ingombro tagli di testa e di coda
|
|
EMC.HOVM = EgtGetInfo( nRawId, 'HOVM', 'd') or 0
|
|
EMC.HCING = 0
|
|
EMC.TCING = EgtGetInfo( nRawId, 'TCING', 'd') or 0
|
|
-- Eseguo preparazione alla rotazione
|
|
local vCmd = SpecCalcPreRot()
|
|
SpecOutputCmds( vCmd, true)
|
|
return
|
|
end
|
|
|
|
-- Se l'operazione successiva è ancora una disposizione, devo scaricare il pezzo
|
|
if nNextOpeType == MCH_OY.DISP and bSplit and not IsEnd2Phase( EMC.PHASE) then
|
|
-- aggiornamento posizioni
|
|
local nPrevOpeId = EgtGetPrevActiveOperation( EMC.DISPID)
|
|
local nLastPathId = EgtGetLastInGroup( EgtGetFirstNameInGroup( nPrevOpeId, 'CL') or GDB_ID.NULL)
|
|
local nLastEntId = EgtGetLastInGroup( nLastPathId)
|
|
local vAxes = EmtGetAxesPos( nLastEntId)
|
|
if #vAxes > 0 then EMC.TPOS = vAxes[1] end
|
|
EMC.YDELTA = EgtGetInfo( nLastPathId, 'YDELTA', 'd')
|
|
EMC.VDELTA = EgtGetInfo( nLastPathId, 'VDELTA', 'd')
|
|
EMC.CNT = EgtGetInfo( nLastPathId, 'CNT', 'i')
|
|
-- Determinazione delle dimensioni del grezzo in lavoro
|
|
local b3Raw = BBox3d()
|
|
local nRawId = EgtGetFirstRawPart()
|
|
while nRawId do
|
|
if EgtVerifyRawPartPhase( nRawId, EMC.PHASE) and not EgtVerifyRawPartPhase( nRawId, EMC.PHASE + 1) then
|
|
b3Raw = EgtGetRawPartBBox( nRawId)
|
|
break
|
|
end
|
|
nRawId = EgtGetNextRawPart( nRawId)
|
|
end
|
|
EMC.LB = b3Raw:getDimX() + 10 * GEO.EPS_SMALL
|
|
EMC.HOVM = EgtGetInfo( nRawId or GDB_ID.NULL, 'HOVM', 'd') or 0
|
|
-- Eseguo scarico
|
|
local vCmd = SpecCalcUnload()
|
|
SpecOutputCmds( vCmd, true)
|
|
return
|
|
end
|
|
|
|
-- Recupero il grezzo in lavoro
|
|
local nCurrRawId = GDB_ID.NULL
|
|
local nRawId = EgtGetFirstRawPart()
|
|
while nRawId do
|
|
if EgtVerifyRawPartPhase( nRawId, EMC.PHASE) and not EgtVerifyRawPartPhase( nRawId, EMC.PHASE + 1) then
|
|
nCurrRawId = nRawId
|
|
end
|
|
nRawId = EgtGetNextRawPart( nRawId)
|
|
end
|
|
|
|
-- Determinazione delle sue dimensioni
|
|
local b3Raw = EgtGetRawPartBBox( nCurrRawId)
|
|
if not b3Raw or b3Raw:isEmpty() then
|
|
EMC.ERR = 11
|
|
return
|
|
end
|
|
EMC.LB = b3Raw:getDimX() + 10 * GEO.EPS_SMALL
|
|
EMC.SB = b3Raw:getDimY()
|
|
EMC.HB = b3Raw:getDimZ()
|
|
EMC.ZMIN = b3Raw:getMin():getZ()
|
|
|
|
-- Aggiorno limiti di presa e tolleranza
|
|
UpdateMinJoinDeltaTol( EMC.SB, EMC.HB, EMC.LB)
|
|
|
|
-- Assegno sovramateriale di testa e ingombro tagli di testa e di coda
|
|
EMC.HOVM = EgtGetInfo( nCurrRawId, 'HOVM', 'd') or 0
|
|
EMC.HCING = EgtGetInfo( nCurrRawId, 'HCING', 'd') or 0
|
|
EMC.TCING = EgtGetInfo( nCurrRawId, 'TCING', 'd') or 0
|
|
|
|
-- correggo area non pinzabile in testa o coda in base alle info sulla lavorazione
|
|
local sNotes = EgtGetMachiningParam( MCH_MP.USERNOTES) or ''
|
|
local dHCING = tonumber( EgtGetValInNotes( sNotes, 'HCING'))
|
|
if dHCING then
|
|
EMC.HCING = max( dHCING, -50)
|
|
end
|
|
local dTCING = tonumber( EgtGetValInNotes( sNotes, 'TCING'))
|
|
if dTCING then
|
|
EMC.TCING = max( dTCING, -50)
|
|
end
|
|
|
|
-- Devo scaricare il pezzo o il grezzo rimasto
|
|
-- Posizione trave
|
|
local dPosT
|
|
|
|
-- Se fase 1 o dopo rotazione eseguo carico con carrello Y
|
|
local vCmd = {}
|
|
if EMC.PHASE == 1 or IsEnd2Phase( EMC.PHASE) then
|
|
dPosT = LoadT
|
|
if IsEnd2Phase( EMC.PHASE) then dPosT = dPosT + TurnerOffs - EMC.HOVM end
|
|
vCmd = SpecCalcLoad( dPosT, 0, min( EMC.LB - MinOther - AGG_LOAD - EMC.HOVM, MaxY - dPosT))
|
|
-- se altrimenti fase successiva alla prima di tipo inizio o rimanenza
|
|
elseif IsStartOrRestPhase( EMC.PHASE) then
|
|
-- recupero posizione trave e quota di aggancio carrello
|
|
dPosT = EgtGetInfo( EMC.DISPID, 'TPOS', 'd')
|
|
local dPosY = EgtGetInfo( EMC.DISPID, 'YPOS', 'd')
|
|
-- se carrello agganciato
|
|
if dPosY then
|
|
-- confermo i nuovi parametri di aggancio
|
|
table.insert( vCmd, { 21, dPosY - dPosT, 0})
|
|
-- recupero CNT
|
|
local nPrevOpeId = EgtGetPrevActiveOperation( EMC.DISPID)
|
|
if EgtGetOperationType( nPrevOpeId) == MCH_OY.DISP and EgtExistsInfo( nPrevOpeId, 'SKIP') then
|
|
nPrevOpeId = EgtGetPrevActiveOperation( nPrevOpeId)
|
|
end
|
|
local nLastPathId = EgtGetLastInGroup( EgtGetFirstNameInGroup( nPrevOpeId, 'CL') or GDB_ID.NULL)
|
|
EMC.CNT = EgtGetInfo( nLastPathId, 'CNT', 'i')
|
|
-- altrimenti è grezzo scaricato al carico e devo ricaricarlo
|
|
else
|
|
vCmd = SpecCalcLoad( dPosT, 0, min( EMC.LB - MinOther - AGG_LOAD, MaxY - dPosT))
|
|
end
|
|
-- altrimenti fase successiva pari
|
|
else
|
|
local dPosV = EgtGetInfo( EMC.DISPID, 'VPOS', 'd')
|
|
EMC.VDELTA = dPosV
|
|
end
|
|
-- Se fase inizio o rimanenza, eseguo scambio per avere solo pinza V
|
|
local vCmd2 = {}
|
|
if IsStartOrRestPhase( EMC.PHASE) or IsEnd2Phase( EMC.PHASE) then
|
|
EMC.TPOS = dPosT
|
|
SpecSetCarrPosFromCmds( vCmd)
|
|
local dDistFront = EgtIf( EMC.LB < abs( MinV - UnloadT), MinJoin, EMC.LB - abs( MinV - UnloadT) + MinJoin + DeltaTol) + EMC.HOVM
|
|
vCmd2 = SpecCalcCarriages( dDistFront, 0)
|
|
if vCmd and #vCmd > 1 and vCmd2 and #vCmd2 > 1 then
|
|
table.insert( vCmd, { 0, 'CARR_MOVE'})
|
|
end
|
|
end
|
|
-- eseguo scarico
|
|
SpecSetCarrPosFromCmds( vCmd2)
|
|
local vCmd3 = SpecCalcUnload()
|
|
-- unisco ed emetto i comandi
|
|
vCmd = EgtJoinTables( vCmd, vCmd2)
|
|
vCmd = EgtJoinTables( vCmd, vCmd3)
|
|
SpecOutputCmds( vCmd, true)
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function OnPostApplyMachining()
|
|
|
|
EgtOutLog( ' Lavorazione : ' .. EgtGetName( EMC.MCHID), 1)
|
|
|
|
-- Inizializzo codice di errore
|
|
EMC.ERR = 0
|
|
|
|
-- Recupero la posizione della trave e dei carrelli al termine della precedente operazione
|
|
local nPrevOpeId = EgtGetPrevActiveOperation( EMC.MCHID)
|
|
-- se precedente operazione non esiste, errore
|
|
if not nPrevOpeId then
|
|
EMC.ERR = 1
|
|
return
|
|
-- se precedente operazione è disposizione
|
|
elseif EgtGetOperationType( nPrevOpeId) == MCH_OY.DISP then
|
|
if EMC.PHASE == 1 or IsFirstMachiningAfterRotation( EMC.MCHID) then
|
|
-- posizioni home
|
|
EMC.TPOS = nil
|
|
EMC.YDELTA = nil
|
|
EMC.VDELTA = nil
|
|
EMC.HCING_IGNORE = true
|
|
EMC.CNT = 1
|
|
elseif IsStartOrRestPhase( EMC.PHASE) then
|
|
-- carico le posizioni
|
|
local dPosT = EgtGetInfo( nPrevOpeId, 'TPOS', 'd')
|
|
local dPosY = EgtGetInfo( nPrevOpeId, 'YPOS', 'd')
|
|
-- se carrello agganciato
|
|
if dPosY then
|
|
EMC.TPOS = dPosT
|
|
EMC.YDELTA = dPosY - dPosT
|
|
EMC.VDELTA = nil
|
|
-- altrimenti è grezzo scaricato al carico e devo ricaricarlo
|
|
else
|
|
EMC.TPOS = nil
|
|
EMC.YDELTA = nil
|
|
EMC.VDELTA = nil
|
|
end
|
|
EMC.HCING_IGNORE = true
|
|
EMC.CNT = nil
|
|
else
|
|
-- aggiornamento posizioni (da lavorazione precedente a disposizione)
|
|
local nPrev2OpeId = EgtGetPrevActiveOperation( nPrevOpeId)
|
|
if not nPrev2OpeId then
|
|
EMC.ERR = 2
|
|
return
|
|
end
|
|
local nLastPathId = EgtGetLastInGroup( EgtGetFirstNameInGroup( nPrev2OpeId, 'CL') or GDB_ID.NULL)
|
|
local nLastEntId = EgtGetLastInGroup( nLastPathId)
|
|
local vAxes = EmtGetAxesPos( nLastEntId or GDB_ID.NULL)
|
|
if not vAxes then
|
|
EMC.ERR = 3
|
|
return
|
|
end
|
|
if #vAxes > 0 then EMC.TPOS = vAxes[1] end
|
|
EMC.YDELTA = EgtGetInfo( nLastPathId, 'YDELTA', 'd')
|
|
EMC.VDELTA = EgtGetInfo( nLastPathId, 'VDELTA', 'd')
|
|
EMC.CNT = nil
|
|
end
|
|
-- altrimenti precedente operazione è lavorazione
|
|
else
|
|
-- aggiornamento posizioni
|
|
local nLastPathId = EgtGetLastInGroup( EgtGetFirstNameInGroup( nPrevOpeId, 'CL') or GDB_ID.NULL)
|
|
local nLastEntId = EgtGetLastInGroup( nLastPathId)
|
|
local vAxes = EmtGetAxesPos( nLastEntId or GDB_ID.NULL)
|
|
if not vAxes then
|
|
EMC.ERR = 4
|
|
return
|
|
end
|
|
if #vAxes > 0 then EMC.TPOS = vAxes[1] end
|
|
EMC.YDELTA = EgtGetInfo( nLastPathId, 'YDELTA', 'd')
|
|
EMC.VDELTA = EgtGetInfo( nLastPathId, 'VDELTA', 'd')
|
|
EMC.CNT = EgtGetInfo( nLastPathId, 'CNT', 'i')
|
|
end
|
|
|
|
-- Verifico se ultima lavorazione della fase
|
|
local nNextOpeId = EgtGetNextActiveOperation( EMC.MCHID)
|
|
local bMchLast = ( not nNextOpeId or EgtGetOperationPhase( nNextOpeId) ~= EMC.PHASE)
|
|
|
|
-- Verifico se ultima lavorazione prima di una rotazione
|
|
local bPreRotMch = IsLastOperationBeforeRotation( EMC.MCHID)
|
|
|
|
-- Verifico flag di separazione e fase di scarico
|
|
local sNotes = EgtGetMachiningParam( MCH_MP.USERNOTES)
|
|
local bPreSplit = ( sNotes:find( 'Presplit') ~= nil)
|
|
local bSplitting = ( sNotes:find( 'Split') ~= nil)
|
|
local bPreCut = ( sNotes:find( 'Precut') ~= nil)
|
|
local bCutting = ( sNotes:find( 'Cut') ~= nil)
|
|
|
|
-- correggo area non pinzabile in testa o coda in base alle info sulla lavorazione
|
|
local dHCING = tonumber( EgtGetValInNotes( sNotes, 'HCING'))
|
|
if dHCING then
|
|
EMC.HCING = max( dHCING, -50)
|
|
end
|
|
local dTCING = tonumber( EgtGetValInNotes( sNotes, 'TCING'))
|
|
if dTCING then
|
|
EMC.TCING = max( dTCING, -50)
|
|
end
|
|
|
|
local bUnload = IsEndPhase( EMC.PHASE) or IsEnd2Phase( EMC.PHASE)
|
|
if bPreSplit or bPreCut then
|
|
EgtSetInfo( EMC.MCHID, 'IS_PRE', '1')
|
|
else
|
|
EgtRemoveInfo( EMC.MCHID, 'IS_PRE')
|
|
end
|
|
|
|
-- Agisco sui diversi percorsi della lavorazione
|
|
local nPathId = EgtGetFirstInGroup( EgtGetFirstNameInGroup( EMC.MCHID, 'CL') or GDB_ID.NULL)
|
|
while nPathId do
|
|
-- assegno id percorso da elaborare
|
|
EMC.PATHID = nPathId
|
|
-- recupero id del successivo
|
|
nPathId = EgtGetNext( nPathId)
|
|
-- verifico se ultimo percorso di ultima lavorazione della fase
|
|
local bLast = ( bMchLast and ( not nPathId))
|
|
-- se ultimo, elimino ritorno in home
|
|
if bLast then EgtRemoveOperationHome( EMC.MCHID) end
|
|
-- salvo lo stato di trave e carrelli
|
|
local OriTPos = EMC.TPOS
|
|
local OriYDelta = EMC.YDELTA
|
|
local OriVDelta = EMC.VDELTA
|
|
local OriCnt = EMC.CNT
|
|
-- eseguo le elaborazioni
|
|
SpecApplyPath( bPreSplit, bSplitting, bPreCut, bCutting, bLast and bUnload, bLast and bPreRotMch)
|
|
-- se separazione, verifico il risultato
|
|
if bSplitting then
|
|
-- recupero CUTID del pezzo in lavoro
|
|
local CutID = GetCUTID()
|
|
-- in caso di errore mancato pinzaggio uscita riprovo dopo aver disabilitato le lavorazioni finali
|
|
if EMC.ERR == 18 then
|
|
-- segnalazione warning
|
|
EMC.ERR = -101
|
|
EMC.MSG = 'Warning : skipped final processes (WRN=101,CUTID='..tostring( CutID)..')'
|
|
-- ripristino lo stato originale di trave e carrelli
|
|
EMC.TPOS = OriTPos
|
|
EMC.YDELTA = OriYDelta
|
|
EMC.VDELTA = OriVDelta
|
|
EMC.CNT = OriCnt
|
|
-- eseguo le elaborazioni
|
|
SpecApplyPath( bPreSplit, bSplitting, bPreCut, bCutting, bLast and bUnload, bLast and bPreRotMch)
|
|
-- pinzaggio ancora impossibile, pezzo a caduta
|
|
if EMC.ERR == 18 then
|
|
-- segnalazione warning
|
|
EMC.ERR = -102
|
|
EMC.MSG = 'Warning : skipped final processes and unload by fall (WRN=102,CUTID='..tostring( CutID)..')'
|
|
end
|
|
-- scarico standard
|
|
elseif EMC.ERR == 0 then
|
|
-- segnalazione warning
|
|
EMC.ERR = -100
|
|
EMC.MSG = 'Warning : standard unload (WRN=100,CUTID='..tostring( CutID)..')'
|
|
end
|
|
-- se taglio del residuo finale, scarico standard
|
|
elseif bCutting then
|
|
-- recupero CUTID del pezzo in lavoro
|
|
local CutID = GetCUTID()
|
|
-- se non ci sono errori, segnalazione warning
|
|
if EMC.ERR == 0 then
|
|
EMC.ERR = -100
|
|
EMC.MSG = 'Warning : standard unload (WRN=100,CUTID='..tostring( CutID)..')'
|
|
end
|
|
end
|
|
if EMC.ERR > 0 then return end
|
|
-- determino la posizione finale della trave
|
|
local nLastEntId = EgtGetLastInGroup( EMC.PATHID)
|
|
local vAxes = EmtGetAxesPos( nLastEntId)
|
|
if #vAxes > 0 then EMC.TPOS = vAxes[1] end
|
|
end
|
|
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function SpecApplyPath( bPreSplit, bSplitting, bPreCut, bCutting, bUnload, bPreRotMch)
|
|
|
|
-- Assegno flag di pezzo separato dal resto del grezzo
|
|
local bSplit = IsEndPhase( EMC.PHASE) or IsMid2Phase( EMC.PHASE) or IsEnd2Phase( EMC.PHASE)
|
|
|
|
-- Assegno flag di pezzo in separazione
|
|
local bFixedDelta = ( bPreSplit or bSplitting or bPreCut or bCutting)
|
|
|
|
-- Se separazione o taglio del grezzo finale, verifico se precedente era una preparazione
|
|
local bFixedPos = false
|
|
if bPreSplit or bSplitting or bPreCut or bCutting then
|
|
local nPrevOpeId = EgtGetPrevActiveOperation( EMC.MCHID)
|
|
bFixedPos = EgtExistsInfo( nPrevOpeId or GDB_ID.NULL, 'IS_PRE')
|
|
end
|
|
|
|
-- Determinazione delle dimensioni totali dei grezzi e del grezzo in lavoro
|
|
local b3Tot = BBox3d()
|
|
local b3Raw = BBox3d()
|
|
local nNextOddPhase = GetNextStartOrRestPhase( EMC.PHASE)
|
|
local nRawId = EgtGetFirstRawPart()
|
|
local nCurrRawId = GDB_ID.NULL
|
|
while nRawId do
|
|
if EgtVerifyRawPartPhase( nRawId, EMC.PHASE) then
|
|
local b3Tmp = EgtGetRawPartBBox( nRawId)
|
|
b3Tot:Add( b3Tmp)
|
|
if EgtGetPartInRawPartCount( nRawId) > 0 and not EgtVerifyRawPartPhase( nRawId, nNextOddPhase) then
|
|
b3Raw = b3Tmp
|
|
nCurrRawId = nRawId
|
|
end
|
|
end
|
|
nRawId = EgtGetNextRawPart( nRawId)
|
|
end
|
|
if b3Tot:isEmpty() then
|
|
EMC.ERR = 11
|
|
return
|
|
end
|
|
EMC.LB = EgtIf( bSplit, b3Raw:getDimX(), b3Tot:getDimX()) + 10 * GEO.EPS_SMALL
|
|
EMC.SB = b3Tot:getDimY()
|
|
EMC.HB = b3Tot:getDimZ()
|
|
EMC.LR = b3Raw:getDimX() + 10 * GEO.EPS_SMALL
|
|
EMC.YMIN = b3Raw:getMin():getY()
|
|
EMC.ZMIN = b3Raw:getMin():getZ()
|
|
|
|
-- Aggiorno limiti di presa e tolleranza
|
|
UpdateMinJoinDeltaTol( EMC.SB, EMC.HB, EMC.LB)
|
|
|
|
-- Recupero sovramateriale di testa e ingombro tagli di testa e di coda
|
|
EMC.HOVM = EgtGetInfo( nCurrRawId, 'HOVM', 'd') or 0
|
|
EMC.HCING = EgtGetInfo( nCurrRawId, 'HCING', 'd') or 0
|
|
EMC.TCING = EgtGetInfo( nCurrRawId, 'TCING', 'd') or 0
|
|
|
|
-- correggo area non pinzabile in testa o coda in base alle info sulla lavorazione
|
|
local sNotes = EgtGetMachiningParam( MCH_MP.USERNOTES)
|
|
local dHCING = tonumber( EgtGetValInNotes( sNotes, 'HCING'))
|
|
if dHCING then
|
|
EMC.HCING = max( dHCING, -50)
|
|
end
|
|
local dTCING = tonumber( EgtGetValInNotes( sNotes, 'TCING'))
|
|
if dTCING then
|
|
EMC.TCING = max( dTCING, -50)
|
|
end
|
|
|
|
-- Calcolo dell'ingombro della lavorazione
|
|
local dDistFront, dDistBack = SpecialCalcMachiningEncumbrance( EMC.MCHID, bPreCut)
|
|
if not dDistFront or not dDistBack then return end
|
|
local dMaxLenLeft = 0
|
|
if bPreSplit or bSplitting then
|
|
local dDistF, dDistB, dMaxLF = SpecialCalcPhaseEncumbrance( EMC.PHASE + 1)
|
|
dDistFront = min( dDistFront, dDistF)
|
|
local dNextHOVM = EgtGetInfo( EgtGetNextRawPart( nCurrRawId) or GDB_ID.NULL, 'HOVM', 'd') or 0
|
|
local dBackOther = b3Tot:getDimX() - b3Raw:getDimX() - MinOther - dNextHOVM
|
|
EgtOutLog( 'DistBack='..EgtNumToString( dDistBack)..' OtherBack='..EgtNumToString( dBackOther), 3)
|
|
dDistBack = min( dDistBack, dBackOther)
|
|
if bSplitting then dMaxLenLeft = dMaxLF end
|
|
elseif bPreCut or bCutting then
|
|
local dDistF = SpecialCalcPhaseEncumbrance( EMC.PHASE + 1)
|
|
dDistFront = min( dDistFront, dDistF)
|
|
dDistBack = 0.0
|
|
end
|
|
|
|
-- Verifico lunghezza pezzo
|
|
if not bSplit and not VerifyPartLength() then
|
|
return
|
|
end
|
|
|
|
-- Se inizio o appena dopo rotazione, eseguo il carico
|
|
if not EMC.TPOS then
|
|
local dPosT = LoadT
|
|
if IsFirstMachiningAfterRotation( EMC.MCHID) then dPosT = dPosT + TurnerOffs end
|
|
local vCmd = SpecCalcLoad( dPosT, dDistFront, max( dDistBack, MinJoin))
|
|
local vCmd2 = SpecCalcCarriages( dDistFront, dDistBack, bFixedDelta, bFixedPos)
|
|
if vCmd2 and #vCmd2 > 1 then
|
|
table.insert( vCmd, { 0, 'CARR_MOVE'})
|
|
end
|
|
EgtJoinTables( vCmd, vCmd2)
|
|
SpecOutputCmds( vCmd)
|
|
|
|
-- Se altrimenti carri entrambi diponibili, eseguo calcoli per carrelli
|
|
elseif not IsEndPhase( EMC.PHASE) then
|
|
local vCmd = SpecCalcCarriages( dDistFront, dDistBack, bFixedDelta, bFixedPos)
|
|
-- Se non ci sono spostamenti, confermo i parametri di aggancio
|
|
if SpecTestOnlyRemarkInCmds( vCmd) then
|
|
table.insert( vCmd, { 21, EgtIf( EMC.YDELTA, EMC.YDELTA, 0), EgtIf( EMC.VDELTA, EMC.VDELTA, 0)})
|
|
end
|
|
SpecOutputCmds( vCmd)
|
|
|
|
-- Altrimenti, non muovo i carrelli rispetto alla trave
|
|
else
|
|
local vCmd = {}
|
|
SpecOutputCmds( vCmd)
|
|
end
|
|
EMC.HCING_IGNORE = nil
|
|
|
|
-- Se taglio di separazione
|
|
local vCmd = {}
|
|
if bSplitting then
|
|
-- rimuovo eventuale vecchia info di Skip
|
|
local NextDispId = EgtGetPhaseDisposition( EMC.PHASE + 1) or GDB_ID.NULL
|
|
EgtRemoveInfo( NextDispId, 'SKIP')
|
|
-- verifico se separazione con caduta
|
|
if not EMC.VDELTA then
|
|
EgtOutLog( ' Warning SPLITTING -> separazione con caduta pezzo')
|
|
if IsEndPhase( EMC.PHASE + 1) then
|
|
EgtSetInfo( NextDispId, 'SKIP', '1')
|
|
local NextOpeId = EgtGetNextOperation( NextDispId)
|
|
while NextOpeId and EgtGetOperationPhase( NextOpeId) == EMC.PHASE + 1 do
|
|
EgtSetOperationMode( NextOpeId, false)
|
|
NextOpeId = EgtGetNextOperation( NextOpeId)
|
|
end
|
|
end
|
|
EMC.ERR = 18
|
|
-- verifico che la barra sia agganciata ad entrambi i carrelli
|
|
elseif not EMC.YDELTA or not EMC.VDELTA then
|
|
EMC.ERR = 19
|
|
EMC.MSG = ' Error SPLIT : Y or V not clamped'
|
|
return false
|
|
end
|
|
-- eseguo la separazione ( standard o di pezzo ruotato)
|
|
EgtOutLog( 'MaxLenLeft=' .. EgtNumToString( dMaxLenLeft, 1), 1)
|
|
if not IsMid2Phase( EMC.PHASE + 1) then
|
|
vCmd = SpecCalcSplit( b3Raw:getDimX(), dMaxLenLeft)
|
|
else
|
|
vCmd = SpecCalcSplitRot( b3Raw:getDimX())
|
|
end
|
|
end
|
|
-- Se taglio finale di grezzo a perdere
|
|
if bCutting then
|
|
-- salvo distanza carrello V da inizio grezzo rimasto nella disposizione della prossima fase
|
|
local NextDispId = EgtGetPhaseDisposition( EMC.PHASE + 1)
|
|
if NextDispId then
|
|
EgtSetInfo( NextDispId, 'VPOS', EMC.VDELTA)
|
|
end
|
|
end
|
|
|
|
-- Se previsto scarico, lo eseguo
|
|
if bUnload then
|
|
EMC.LB = b3Raw:getDimX() + 10 * GEO.EPS_SMALL
|
|
local vCmdTmp = SpecCalcUnload()
|
|
vCmd = EgtJoinTables( vCmd, vCmdTmp)
|
|
end
|
|
|
|
-- Se ritorno al carico per rotazione
|
|
if bPreRotMch then
|
|
-- determino posizione testa trave
|
|
local nLastEntId = EgtGetLastInGroup( EMC.PATHID)
|
|
local vAxes = EmtGetAxesPos( nLastEntId)
|
|
if #vAxes > 0 then EMC.TPOS = vAxes[1] end
|
|
-- eseguo movimento prima di rotazione
|
|
local vCmdTmp = SpecCalcPreRot()
|
|
vCmd = EgtJoinTables( vCmd, vCmdTmp)
|
|
end
|
|
|
|
-- Emetto eventuali comandi di separazione e/o scarico
|
|
if #vCmd > 0 then
|
|
SpecOutputCmds( vCmd, true)
|
|
end
|
|
|
|
end --SpecApplyPath( bLast)
|
|
|
|
---------------------------------------------------------------------
|
|
function SpecialCalcMachiningEncumbrance( nMchId, bPreCut)
|
|
-- gruppi della lavorazione
|
|
local nClId = EgtGetFirstNameInGroup( nMchId, 'CL')
|
|
local nPathId = EgtGetFirstInGroup( nClId or GDB_ID.NULL)
|
|
if not nPathId then
|
|
EMC.ERR = 12
|
|
return
|
|
end
|
|
-- recupero ptMin ptMax della lavorazione
|
|
local ptMin = EgtGetInfo( nClId, 'MMIN', 'p')
|
|
local ptMax = EgtGetInfo( nClId, 'MMAX', 'p')
|
|
if not ptMin or not ptMax then
|
|
EMC.ERR = 13
|
|
return
|
|
end
|
|
-- se pre-taglio, aggiorno ptMax con quello del taglio finale
|
|
if bPreCut then
|
|
local ptFinMax = GetFinalCutPmax( nMchId)
|
|
if ptFinMax then
|
|
ptMax = ptFinMax
|
|
end
|
|
end
|
|
-- Recupero del vettore estrusione (coincide con il vettore utensile)
|
|
local vtTool = EgtGetInfo( nPathId, 'EXTR', 'v')
|
|
if not vtTool then
|
|
EMC.ERR = 14
|
|
return
|
|
end
|
|
-- Recupero testa
|
|
local sHead = EgtTdbGetCurrToolParam( MCH_TP.HEAD)
|
|
-- Calcolo del vettore ausiliario
|
|
local vAxes = EmtGetAxesPos( EgtGetFirstInGroup( nPathId))
|
|
if not vAxes or #vAxes < 5 or ( sHead == 'H3' and #vAxes < 6) then
|
|
EMC.ERR = 15
|
|
return
|
|
end
|
|
local vtAux = EgtGetCalcAuxDirFromAngles( vAxes[4], vAxes[5], vAxes[6])
|
|
if not vtAux then
|
|
EMC.ERR = 16
|
|
return
|
|
end
|
|
-- Recupero dei dati dell'utensile
|
|
local nToolType = EgtTdbGetCurrToolParam( MCH_TP.TYPE)
|
|
local bSaw = ( nToolType == MCH_TY.SAW_STD or nToolType == MCH_TY.SAW_FLAT)
|
|
local bChain = ( nToolType == MCH_TY.MORTISE_STD)
|
|
local bAngTrasm = ( sHead == 'H5' or sHead == 'H6')
|
|
local dTLen = EgtTdbGetCurrToolParam( MCH_TP.LEN)
|
|
local dTDiam = EgtTdbGetCurrToolParam( MCH_TP.DIAM)
|
|
local dThLen = EgtTdbGetCurrToolThLength()
|
|
-- Se sega a catena, devo correggere il versore Aux per farlo coincidere con la direzione del braccio C
|
|
if bChain then
|
|
if abs( vAxes[6] or 0) < 1 then
|
|
vtAux = vtTool
|
|
else
|
|
vtAux = vtTool ^ vtAux
|
|
end
|
|
end
|
|
-- Se rinvio da sotto, devo correggere il versore Aux per farlo coincidere con la direzione del braccio C
|
|
if bAngTrasm then
|
|
local nExit = EgtTdbGetCurrToolParam( MCH_TP.EXIT)
|
|
vtAux = vtTool ^ vtAux
|
|
if nExit == 2 then vtAux = -vtAux end
|
|
end
|
|
-- Calcolo limiti derivanti dalla lavorazione
|
|
local dDistFront, dDistBack = SpecCalcEncumbrance( vtTool, vtAux, ptMin, ptMax, bSaw, bChain, dTLen, dTDiam, dThLen)
|
|
return dDistFront, dDistBack
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function SpecialCalcPhaseEncumbrance( nPhase)
|
|
-- Deve essere la fase finale di lavorazione di un pezzo (già staccato dal resto della trave)
|
|
local dDistFront = EMC.LB
|
|
local dDistBack = EMC.LB
|
|
local dMaxLenLeft = 0
|
|
-- Salvo lavorazione e utensile correnti, per ripristinarli alla fine
|
|
local nOrigMchId = EgtGetCurrMachining()
|
|
local sOrigTool = EgtTdbGetCurrToolParam( MCH_TP.NAME)
|
|
local sOrigHead = EgtTdbGetCurrToolParam( MCH_TP.HEAD)
|
|
local nOrigExit = EgtTdbGetCurrToolParam( MCH_TP.EXIT)
|
|
-- Ciclo sulle lavorazioni
|
|
local nMchId = EgtGetNextActiveOperation( EgtGetPhaseDisposition( nPhase) or GDB_ID.NULL)
|
|
while nMchId and EgtGetOperationPhase( nMchId) == nPhase do
|
|
-- imposto lavorazione e utensile correnti
|
|
EgtSetCurrMachining( nMchId)
|
|
local sTool = EgtGetMachiningParam( MCH_MP.TOOL)
|
|
if not EgtTdbSetCurrTool( sTool) then
|
|
local sTuuid = EgtGetMachiningParam( MCH_MP.TUUID)
|
|
sTool = EgtTdbGetToolFromUUID( sTuuid)
|
|
EgtTdbSetCurrTool( sTool)
|
|
end
|
|
local sHead = EgtTdbGetCurrToolParam( MCH_TP.HEAD)
|
|
local nExit = EgtTdbGetCurrToolParam( MCH_TP.EXIT)
|
|
if sTool and sHead and nExit then EgtSetCalcTool( sTool, sHead, nExit) end
|
|
-- calcolo ingombri
|
|
local dDistF, dDistB = SpecialCalcMachiningEncumbrance( nMchId)
|
|
if dDistF and dDistB then
|
|
dDistFront = min( dDistFront, dDistF)
|
|
dDistBack = min( dDistBack, dDistB)
|
|
local dMaxLenL = EMC.LR - dDistF
|
|
EgtOutLog( ' MaxLenLeft = ' .. EgtNumToString( dMaxLenL, 1), 3)
|
|
dMaxLenLeft = max( dMaxLenLeft, dMaxLenL)
|
|
end
|
|
nMchId = EgtGetNextActiveOperation( nMchId)
|
|
end
|
|
-- Ripristino lavorazione e utensile correnti
|
|
if nOrigMchId then EgtSetCurrMachining( nOrigMchId) end
|
|
if sOrigTool then EgtTdbSetCurrTool( sOrigTool) end
|
|
if sOrigTool and sOrigHead and nOrigExit then EgtSetCalcTool( sOrigTool, sOrigHead, nOrigExit) end
|
|
-- Restituisco gli ingombri trovati
|
|
return dDistFront, dDistBack, dMaxLenLeft
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function GetFinalCutPmax( nMchId)
|
|
local nFinalCutId
|
|
local nId = EgtGetNextActiveOperation( nMchId)
|
|
while nId and EgtGetOperationPhase( nId) == EMC.PHASE do
|
|
nFinalCutId = nId
|
|
nId = EgtGetNextActiveOperation( nId)
|
|
end
|
|
if not nFinalCutId then return end
|
|
local nCLId = EgtGetFirstNameInGroup( nFinalCutId, 'CL')
|
|
if not nCLId then return end
|
|
return EgtGetInfo( nCLId, 'MMAX', 'p')
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function SpecCalcEncumbrance( vtTool, vtArm, ptMin, ptMax, bSaw, bChain, dTLen, dTDiam, dThLen)
|
|
-- Quota in Z dal punto di inclinazione dei carrelli
|
|
local dCompZ = sqrt( 1 - vtTool:getZ() * vtTool:getZ())
|
|
local dZup = ptMin:getZ() - 0.5 * dCompZ * dTDiam - ( EMC.ZMIN + 130)
|
|
-- Posizione min e max del naso mandrino (rispetto a riferimento pezzo in Y e Z)
|
|
local ptHeadMin = ptMin + vtTool * dTLen - Vector3d( 0, EMC.YMIN + EMC.SB, EMC.ZMIN)
|
|
local ptHeadMax = ptMax + vtTool * dTLen - Vector3d( 0, EMC.YMIN + EMC.SB, EMC.ZMIN)
|
|
-- Ingombro a sinistra
|
|
local dDistBack = EMC.LB + ptMin:getX() + LoadT
|
|
local dHeadBack = 350
|
|
if bSaw then
|
|
if vtTool:getX() > 0 and abs( vtTool:getY()) < 0.088 and abs( vtTool:getZ()) < 0.088 then
|
|
dHeadBack = 50 + 0.5 * dTDiam * sqrt( 1 - vtTool:getX() * vtTool:getX()) + EgtIf( EMC.CNT == 1, 10, 0)
|
|
elseif abs( vtTool:getZ()) < 0.26 and abs( vtTool:getX()) < 0.35 then
|
|
dHeadBack = EgtIf( vtArm:getX() < 0, 540, 350)
|
|
elseif abs( vtTool:getZ()) < 0.26 and abs( vtTool:getX()) < 0.71 then
|
|
if vtArm:getX() < 0 then
|
|
dHeadBack = 450
|
|
else
|
|
dHeadBack = EgtIf( vtTool:getX() > 0, 50, 90) + 0.5 * dTDiam * sqrt( 1 - vtTool:getX() * vtTool:getX())
|
|
end
|
|
elseif ( vtTool:getX() > 0.7 and abs( vtTool:getY()) < 0.2 and ptMax:getZ() > EMC.ZMIN + 0.9 * EMC.HB) then
|
|
if vtTool:getZ() > 0 then
|
|
dHeadBack = max( 50, 90 - 0.5 * dTDiam * sqrt( 1 - vtTool:getX() * vtTool:getX()))
|
|
else
|
|
dHeadBack = max( 50, 40 + 0.5 * dTDiam * sqrt( 1 - vtTool:getX() * vtTool:getX()))
|
|
end
|
|
if ptMax:getZ() < EMC.ZMIN + BD.VICE_MINH then
|
|
dHeadBack = dHeadBack + BD.VICE_MINH
|
|
end
|
|
elseif ( vtTool:getX() > 0.2 and abs( vtTool:getZ()) < 0.5) then
|
|
dHeadBack = max( 90, 40 + 0.5 * dTDiam * sqrt( 1 - vtTool:getX() * vtTool:getX()))
|
|
elseif abs( vtTool:getZ()) < 0.93 then
|
|
if vtTool:getX() > 0 and abs( vtTool:getY()) < 0.2 and ptMax:getZ() > EMC.ZMIN + BD.VICE_MINH then
|
|
dHeadBack = 180
|
|
elseif vtTool:getX() > -0.06 then
|
|
dHeadBack = 50 + 0.5 * dTDiam * sqrt( 1 - vtTool:getX() * vtTool:getX())
|
|
elseif vtTool:getX() > -0.3 then
|
|
dHeadBack = 250
|
|
elseif vtTool:getX() > -0.707 then
|
|
dHeadBack = 350
|
|
elseif vtTool:getX() > -0.8667 then
|
|
dHeadBack = 450
|
|
else
|
|
dHeadBack = 660
|
|
end
|
|
else
|
|
dHeadBack = 50 + 0.5 * dTDiam * sqrt( 1 - vtTool:getX() * vtTool:getX())
|
|
end
|
|
else
|
|
if ( vtTool:getX() > -0.1 and vtArm:getX() > -0.1) or
|
|
( abs( vtTool:getX()) < 0.1 and abs( vtTool:getZ()) < 0.1) then
|
|
dHeadBack = EgtIf( EMC.CNT == 1, 180, 130)
|
|
elseif ( vtTool:getX() > -0.1 and vtArm:getX() > -0.95) then
|
|
dHeadBack = 180
|
|
elseif ( vtTool:getX() < -0.8) then
|
|
dHeadBack = 675
|
|
elseif ( vtTool:getX() < -0.75) then
|
|
dHeadBack = 650
|
|
elseif ( vtTool:getX() < -0.5) then
|
|
dHeadBack = 450
|
|
end
|
|
if vtTool:getX() < -0.25 then
|
|
dHeadBack = dHeadBack + max( dTLen - 130, 0) * abs( vtTool:getX())
|
|
elseif vtTool:getX() < 0 then
|
|
dHeadBack = dHeadBack + ( dTLen + 180) * abs( vtTool:getX())
|
|
end
|
|
if vtTool:getX() > 0.866 then
|
|
dHeadBack = 50
|
|
elseif vtTool:getX() >= 0 and dZup > 0 then
|
|
dHeadBack = max( EgtIf( EMC.CNT == 1, 180, 130), dHeadBack - dZup)
|
|
end
|
|
if abs( vtTool:getX()) < 0.5 and abs( vtTool:getZ()) > 0.259 and dZup < 0 then
|
|
if vtArm:getX() < -0.259 then
|
|
dHeadBack = 510
|
|
else
|
|
dHeadBack = EgtIf( vtTool:getZ() > 0.966, 160, 280)
|
|
end
|
|
end
|
|
-- per fresature longitudinali con utensile di fianco
|
|
if abs( vtTool:getX()) < 0.1 and vtTool:getZ() < 0.707 and vtArm:getX() < -0.5 then
|
|
dHeadBack = 500
|
|
end
|
|
-- per sega a catena di fianco
|
|
if bChain and vtTool:getX() < 0.5 and vtTool:getZ() < 0.5 and vtArm:getX() < -0.5 then
|
|
dHeadBack = max( dHeadBack, 510)
|
|
end
|
|
-- per fresa diretta quasi esattamente come Y+/- e con la testa non troppo nel pezzo
|
|
if not bChain and abs( vtTool:getX()) < 0.017 and abs( vtTool:getZ()) < 0.017 and
|
|
(( vtTool:getY() > 0 and ptHeadMin:getY() > 80) or ( vtTool:getY() < 0 and ptHeadMax:getY() < -EMC.SB - 80)) then
|
|
dHeadBack = EgtIf( EMC.CNT == 1, 180, 130)
|
|
end
|
|
-- per interferenza con triangolo di rinforzo sulla pinza Y
|
|
if ( vtTool:getZ() < 0.25 and vtTool:getX() < 0.10 and vtTool:getY() > 0 and vtArm:getX() < -0.707 and not BD.RIGHT_LOAD) or
|
|
( vtTool:getZ() < 0.25 and vtTool:getX() < 0.10 and vtTool:getY() < 0 and vtArm:getX() < -0.707 and BD.RIGHT_LOAD)then
|
|
dHeadBack = dHeadBack + 90
|
|
end
|
|
end
|
|
-- Ingombro a destra
|
|
local dDistFront = - ptMax:getX() - LoadT
|
|
local dHeadFront = 350
|
|
if bSaw then
|
|
if vtTool:getX() < 0 and abs( vtTool:getY()) < 0.088 and abs( vtTool:getZ()) < 0.088 then
|
|
dHeadFront = 50 + 0.5 * dTDiam * sqrt( 1 - vtTool:getX() * vtTool:getX()) + EgtIf( EMC.CNT == 1, 10, 0)
|
|
elseif abs( vtTool:getZ()) < 0.26 and abs( vtTool:getX()) < 0.35 then
|
|
dHeadFront = EgtIf( vtArm:getX() > 0, 540, 350)
|
|
elseif abs( vtTool:getZ()) < 0.26 and abs( vtTool:getX()) < 0.71 then
|
|
if vtArm:getX() > 0 then
|
|
dHeadFront = 450
|
|
else
|
|
dHeadFront = EgtIf( vtTool:getX() < 0, 50, 90) + 0.5 * dTDiam * sqrt( 1 - vtTool:getX() * vtTool:getX())
|
|
end
|
|
elseif ( vtTool:getX() < - 0.7 and abs( vtTool:getY()) < 0.2 and ptMax:getZ() > EMC.ZMIN + 0.9 * EMC.HB) then
|
|
if vtTool:getZ() > 0 then
|
|
dHeadFront = max( 50, 90 - 0.5 * dTDiam * sqrt( 1 - vtTool:getX() * vtTool:getX()))
|
|
else
|
|
dHeadFront = max( 50, 40 + 0.5 * dTDiam * sqrt( 1 - vtTool:getX() * vtTool:getX()))
|
|
end
|
|
if ptMax:getZ() < EMC.ZMIN + BD.VICE_MINH then
|
|
dHeadFront = dHeadFront + BD.VICE_MINH
|
|
end
|
|
elseif ( vtTool:getX() < -0.2 and abs( vtTool:getZ()) < 0.5) then
|
|
dHeadFront = max( 90, 40 + 0.5 * dTDiam * sqrt( vtTool:getY() * vtTool:getY() + vtTool:getZ() * vtTool:getZ()))
|
|
elseif abs( vtTool:getZ()) < 0.93 then
|
|
if vtTool:getX() < 0 and abs( vtTool:getY()) < 0.2 and ptMax:getZ() > EMC.ZMIN + BD.VICE_MINH then
|
|
dHeadFront = 180
|
|
elseif vtTool:getX() < 0.06 then
|
|
dHeadFront = 50 + 0.5 * dTDiam * sqrt( 1 - vtTool:getX() * vtTool:getX())
|
|
elseif vtTool:getX() < 0.3 then
|
|
dHeadFront = 250
|
|
elseif vtTool:getX() < 0.707 then
|
|
dHeadFront = 350
|
|
elseif vtTool:getX() < 0.8667 then
|
|
dHeadFront = 450
|
|
else
|
|
dHeadFront = 660
|
|
end
|
|
else
|
|
dHeadFront = 50 + 0.5 * dTDiam * sqrt( 1 - vtTool:getX() * vtTool:getX())
|
|
end
|
|
else
|
|
if ( vtTool:getX() < -0.5 and vtArm:getX() < 0.1) then
|
|
dHeadFront = max( 50, dTDiam / 2 * abs( vtTool:getZ()) + 20)
|
|
elseif ( vtTool:getX() < 0.1 and vtArm:getX() < 0.1) or
|
|
( abs( vtTool:getX()) < 0.1 and abs( vtTool:getZ()) < 0.1) then
|
|
dHeadFront = 130
|
|
elseif ( vtTool:getX() < 0.1 and vtArm:getX() < 0.95) then
|
|
dHeadFront = 180
|
|
elseif ( vtTool:getX() > 0.8) then
|
|
dHeadFront = 675
|
|
elseif ( vtTool:getX() > 0.75) then
|
|
dHeadFront = 650
|
|
elseif ( vtTool:getX() > 0.5) then
|
|
dHeadFront = 450
|
|
end
|
|
if vtTool:getX() > 0.25 then
|
|
dHeadFront = dHeadFront + max( dTLen - 130, 0) * vtTool:getX()
|
|
elseif vtTool:getX() > 0 then
|
|
dHeadFront = dHeadFront + ( dTLen + 180) * vtTool:getX()
|
|
end
|
|
if vtTool:getX() < -0.866 then
|
|
dHeadFront = 50
|
|
elseif vtTool:getX() <= 0 and dZup > 0 then
|
|
dHeadFront = max( 130, dHeadFront - dZup)
|
|
end
|
|
if abs( vtTool:getX()) < 0.5 and abs( vtTool:getZ()) > 0.259 and dZup < 0 then
|
|
if vtArm:getX() > 0.259 then
|
|
dHeadFront = 510
|
|
else
|
|
dHeadFront = EgtIf( vtTool:getZ() > 0.966, 170, 280)
|
|
end
|
|
end
|
|
-- per fresature longitudinali con utensile di fianco
|
|
if abs( vtTool:getX()) < 0.1 and vtTool:getZ() < 0.707 and vtArm:getX() > 0.5 then
|
|
dHeadFront = 500
|
|
end
|
|
-- per sega a catena di fianco
|
|
if bChain and vtTool:getX() > -0.5 and vtTool:getZ() < 0.5 and vtArm:getX() > 0.5 then
|
|
dHeadFront = max( dHeadFront, 510)
|
|
end
|
|
-- per fresa diretta quasi esattamente come Y+/- e con la testa non troppo nel pezzo
|
|
if not bChain and abs( vtTool:getX()) < 0.017 and abs( vtTool:getZ()) < 0.017 and
|
|
(( vtTool:getY() > 0 and ptHeadMin:getY() > 80) or ( vtTool:getY() < 0 and ptHeadMax:getY() < -EMC.SB - 80)) then
|
|
dHeadFront = 130
|
|
end
|
|
-- per forature di coda abbastanza lontane dai bordi pezzo in Y
|
|
if BD.ENABLE_TOOL_BETWEEN_VICES and not bChain and vtTool:getX() < -0.999 and ptHeadMax:getY() + dTDiam / 2 < -15 and ptHeadMin:getY() - dTDiam / 2 > -EMC.SB + 15 then
|
|
dHeadFront = - ( dTLen - dThLen - 15)
|
|
end
|
|
-- per interferenza con triangolo di rinforzo sulla pinza V
|
|
if ( vtTool:getZ() < 0.25 and vtTool:getX() > -0.10 and vtTool:getY() > 0 and vtArm:getX() > 0.707 and not BD.RIGHT_LOAD) or
|
|
( vtTool:getZ() < 0.25 and vtTool:getX() > -0.10 and vtTool:getY() < 0 and vtArm:getX() > 0.707 and BD.RIGHT_LOAD) then
|
|
dHeadFront = dHeadFront + 90
|
|
end
|
|
end
|
|
-- Stampe debug
|
|
EgtOutLog( ' Tdir=' .. tostring( vtTool) .. ' Adir=' .. tostring( vtArm) .. ' Zup=' .. EgtNumToString( dZup), 3)
|
|
EgtOutLog( ' DistFront=' .. EgtNumToString( dDistFront) .. ' DistBack=' .. EgtNumToString( dDistBack) ..
|
|
' HeadFront=' .. EgtNumToString( dHeadFront) .. ' HeadBack=' .. EgtNumToString( dHeadBack), 3)
|
|
-- Restituisco ingombri effettivi
|
|
return ( dDistFront - dHeadFront), ( dDistBack - dHeadBack)
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function VerifyPartLength()
|
|
|
|
-- Verifico lunghezza pezzo
|
|
if EMC.LB < MinJoin + MinOther + AGG_LOAD + EMC.HCING + EMC.HOVM then
|
|
EgtOutLog( ' Error CLAMP -> pezzo troppo corto')
|
|
EMC.ERR = 17
|
|
return false
|
|
end
|
|
|
|
return true
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function SpecCalcLoad( dPosT, dDistFront, dDistBack)
|
|
--[L]
|
|
local dMinDistBack= max( dDistBack, MinJoin + EgtIf( IsMid2Phase( EMC.PHASE) or IsEnd2Phase( EMC.PHASE), EMC.TCING, 0))
|
|
local dNewYDelta = max( EMC.LB - dMinDistBack, MinOther + AGG_LOAD + EMC.HCING + EMC.HOVM)
|
|
local dNewVDelta = nil
|
|
local dNewY = dPosT + TurnerOffs + dNewYDelta
|
|
local vCmd = {}
|
|
|
|
-- se flag attivo, vado a pinzare al massimo della corsa
|
|
if BD.FASTCLAMPING then
|
|
EgtOutLog( ' *[L]', 1)
|
|
-- [L-1]
|
|
if dNewY - MaxY > 0 then
|
|
dNewYDelta = min( EMC.LB - MinJoin + AGG_LOAD, MaxY - dPosT - TurnerOffs)
|
|
EgtOutLog( ' *[L1]', 1)
|
|
end --[L-2]
|
|
if EMC.LB - dNewYDelta < MinJoin then
|
|
dNewYDelta = min( EMC.LB - MinJoin + AGG_LOAD, MaxY - dPosT - TurnerOffs)
|
|
EgtOutLog( ' *[L2]', 1)
|
|
end
|
|
else
|
|
-- altrimenti si pinza sempre a massimo 1000mm
|
|
dNewYDelta = min( dNewYDelta, 1000)
|
|
end
|
|
|
|
-- Commento
|
|
table.insert( vCmd, { 0, 'Loading'})
|
|
-- risalita testa a Zmax
|
|
local bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
-- Apro entrambe le morse
|
|
table.insert( vCmd, { 11, 0})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- Sposto il carrello Y per il carico
|
|
table.insert( vCmd, { 2, 'Y', dPosT + dNewYDelta, 'V', ParkV})
|
|
-- Chiudo morsa Y
|
|
table.insert( vCmd, { 11, EgtIf( EMC.LB - dNewYDelta < LenToPress, 1, 2)})
|
|
-- confermo i nuovi parametri di aggancio
|
|
table.insert( vCmd, { 21, dNewYDelta, 0})
|
|
-- Inizializzo contatore globale
|
|
EMC.CNT = 1
|
|
SpecOutputCNT()
|
|
-- Assegno stato corrente
|
|
EMC.TPOS = dPosT
|
|
EMC.YDELTA = dNewYDelta
|
|
EMC.VDELTA = nil
|
|
-- Restituisco i comandi
|
|
return vCmd
|
|
end -- SpecAdjustLoad [L]
|
|
|
|
|
|
---------------------------------------------------------------------
|
|
function GetMachiningStartingPoint( idPath)
|
|
local idFirstObj = EgtGetFirstInGroup( idPath)
|
|
-- se esiste l'oggetto, recupero la coordinata del primo punto
|
|
if idFirstObj then
|
|
local vAxes = EmtGetAxesPos( idFirstObj)
|
|
if #vAxes > 0 then
|
|
return vAxes[1]
|
|
end
|
|
end
|
|
return nil
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function AdjustPositionsForB( dNewYDelta, dNewVDelta, TCING, HCING, HOVM, bFixedDelta)
|
|
local REF_DIST = 1400
|
|
local dYDelta = 0
|
|
local dVDelta = 0
|
|
-- incremento se possibile la distanza tra le due posizioni
|
|
if not bFixedDelta and ( dNewYDelta - dNewVDelta) < REF_DIST then
|
|
local dEffAddDist = ( REF_DIST - ( dNewYDelta - dNewVDelta)) / 2
|
|
dYDelta = max( min( dEffAddDist, EMC.LB - dNewYDelta - MinJoin - TCING), 0)
|
|
dVDelta = max( min( dEffAddDist, dNewVDelta - MinJoin - HOVM - HCING), 0)
|
|
end
|
|
return dYDelta, dVDelta
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function GetDeltaTol( dLenPresa, TCING, HCING, HOVM, Carr, bFixedDelta)
|
|
local dDeltaTolEff = DELTA_SIC
|
|
if Carr == 'Y' then
|
|
local dLenPreEff = dLenPresa - TCING
|
|
if dLenPreEff < MinJoin + DeltaTol then
|
|
dDeltaTolEff = max( DELTA_SIC, dLenPreEff - MinJoin + 10 * GEO.EPS_SMALL)
|
|
else
|
|
if BD.GO_FAST and BD.GO_FAST ~= 0 then
|
|
if bFixedDelta == false then
|
|
dDeltaTolEff = min( EMC.LB + 700, dLenPreEff, 3200)
|
|
else
|
|
dDeltaTolEff = 900
|
|
end
|
|
else
|
|
dDeltaTolEff = DeltaTol
|
|
end
|
|
end
|
|
elseif Carr == 'V' then
|
|
local dLenPreEff = dLenPresa - HCING - HOVM
|
|
if dLenPreEff < MinJoin + DeltaTol then
|
|
dDeltaTolEff = max( DELTA_SIC, dLenPreEff - MinJoin + 10 * GEO.EPS_SMALL)
|
|
else
|
|
dDeltaTolEff = DeltaTol
|
|
end
|
|
end
|
|
return EgtIf( bFixedDelta, min( dDeltaTolEff, DELTA_TOL_FIXED), dDeltaTolEff)
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function SpecCalcCarriages( dDistFront, dDistBack, bFixedDelta, bFixedPos)
|
|
|
|
-- Abilitazione gestione vecchia dei riposizionamenti
|
|
if not BD.NEWCLAMPING then
|
|
return SpecCalcCarriagesOLD( dDistFront, dDistBack, bFixedDelta, bFixedPos)
|
|
end
|
|
|
|
local MinFrontJoin = MinJoin + EMC.HCING + EMC.HOVM
|
|
local MinBackJoin = MinJoin + EgtIf( IsMid2Phase( EMC.PHASE) or IsEnd2Phase( EMC.PHASE), EMC.TCING, 0)
|
|
local MyMinOther = MinOther + EgtIf( EMC.CNT == 1, AGG_LOAD, 0)
|
|
|
|
local dDistFrontEff = min( dDistFront, EMC.LB - MyMinOther - EMC.TCING)
|
|
if bFixedDelta and IsMid2Phase( EMC.PHASE + 1) then
|
|
dDistFrontEff = min( dDistFrontEff, EMC.LR - MinOther - EMC.TCING)
|
|
end
|
|
local dDistBackEff = min( dDistBack, EMC.LB - MyMinOther - EMC.HCING - EMC.HOVM)
|
|
|
|
-- se primo scambio
|
|
local MyMinY = EgtIf( EMC.CNT == 1, MinY + AGG_LOAD, MinY)
|
|
|
|
EgtOutLog( ' Dist/Min : Back=' .. EgtNumToString( dDistBackEff, 1) .. '/' .. EgtNumToString( MinBackJoin, 1) ..
|
|
' Front=' .. EgtNumToString( dDistFrontEff, 1) .. '/' .. EgtNumToString( MinFrontJoin, 1) ..
|
|
' Fixed : Delta=' .. EgtIf( bFixedDelta, 'T', 'F') .. ' Pos=' .. EgtIf( bFixedPos, 'T', 'F'), 3)
|
|
|
|
local dPosT = EMC.TPOS
|
|
local dYDelta = EMC.YDELTA
|
|
local dVDelta = EMC.VDELTA
|
|
local dNewPosT
|
|
-- se sono già settati, non calcolo posizione finale
|
|
if not EMC.YDELTANEXT and not EMC.VDELTANEXT then
|
|
dNewPosT = GetMachiningStartingPoint( EMC.PATHID)
|
|
end
|
|
local dNewYDelta = nil
|
|
local dNewVDelta = nil
|
|
|
|
-- [A] se posso mettere solo carrello Y
|
|
if dDistFrontEff < MinFrontJoin and dDistBackEff > MinBackJoin - GEO.EPS_SMALL then
|
|
-- se Y non era in presa, mi ricalcolo la posizione
|
|
if not dYDelta then
|
|
dYDelta = dVDelta + ( MyMinY - MaxV)
|
|
end
|
|
dNewYDelta = EMC.LB - dDistBackEff
|
|
-- verifico se posso lasciare la morsa in posizione
|
|
local dYDeltaTol = GetDeltaTol( EMC.LB - dNewYDelta, EMC.TCING, EMC.HCING, EMC.HOVM, 'Y')
|
|
local bYDeltaS = ( dNewYDelta < dYDelta - dYDeltaTol or dNewYDelta > dYDelta + DELTA_SIC)
|
|
if bYDeltaS then
|
|
dNewYDelta = dNewYDelta + dYDeltaTol/2
|
|
else
|
|
dNewYDelta = dYDelta
|
|
end
|
|
|
|
dNewVDelta = nil
|
|
-- [B] se altrimenti posso mettere entrambi i carrelli Y e V
|
|
elseif dDistBackEff > MinBackJoin - GEO.EPS_SMALL and dDistFrontEff > MinFrontJoin - GEO.EPS_SMALL then
|
|
-- se Y non era in presa, mi ricalcolo la posizione
|
|
if not dYDelta then
|
|
dYDelta = dVDelta + ( MyMinY - MaxV)
|
|
end
|
|
-- se V non era in presa, mi ricalcolo la posizione
|
|
if not dVDelta then
|
|
dVDelta = dYDelta - ( MyMinY - MaxV)
|
|
end
|
|
|
|
dNewYDelta = EMC.LB - dDistBackEff
|
|
dNewVDelta = dDistFrontEff
|
|
|
|
-- incremento la distanza tra le due posizioni ( se abilitato e possibile)
|
|
local dYDeltaAgg, dVDeltaAgg = AdjustPositionsForB( dNewYDelta, dNewVDelta, EMC.TCING, EMC.HCING, EMC.HOVM, bFixedDelta)
|
|
-- tolleranze
|
|
local dYDeltaTol = dYDeltaAgg + GetDeltaTol( EMC.LB - dNewYDelta - dYDeltaAgg, EMC.TCING, EMC.HCING, EMC.HOVM, 'Y', bFixedDelta)
|
|
local dVDeltaTol = dVDeltaAgg + GetDeltaTol( dNewVDelta - dVDeltaAgg, EMC.TCING, EMC.HCING, EMC.HOVM, 'V', bFixedDelta)
|
|
-- definisco criteri per movimenti 'significativi' in base alle 'nuove' tolleranze
|
|
local bYDeltaS = ( dNewYDelta < dYDelta - dYDeltaTol or dNewYDelta > dYDelta + DELTA_SIC)
|
|
local bVDeltaS = ( dNewVDelta > dVDelta + dVDeltaTol or dNewVDelta < dVDelta - DELTA_SIC)
|
|
-- reimposto i delta finali in caso di spostamenti richiesti significativi, tenuto conto delle tolleranze
|
|
if bYDeltaS then
|
|
dNewYDelta = dNewYDelta + dYDeltaTol / 2
|
|
else
|
|
dNewYDelta = dYDelta
|
|
end
|
|
if bVDeltaS then
|
|
dNewVDelta = dNewVDelta - dVDeltaTol / 2
|
|
else
|
|
dNewVDelta = dVDelta
|
|
end
|
|
|
|
-- [C] se altrimenti posso mettere solo carrello V
|
|
elseif dDistBackEff < MinBackJoin and dDistFrontEff > MinFrontJoin - GEO.EPS_SMALL then
|
|
-- se V non era in presa, mi ricalcolo la posizione
|
|
if not dVDelta then
|
|
dVDelta = dYDelta - ( MyMinY - MaxV)
|
|
end
|
|
dNewVDelta = dDistFrontEff
|
|
-- verifico se posso lasciare la morsa in posizione
|
|
local dVDeltaTol = GetDeltaTol( dNewVDelta, EMC.TCING, EMC.HCING, EMC.HOVM, 'V', bFixedDelta)
|
|
local bVDeltaS
|
|
if EMC.YDELTA == nil and EMC.YDELTANEXT == nil then
|
|
bVDeltaS = (( dNewVDelta > dVDelta + dVDeltaTol and not bFixedPos) or dNewVDelta < dVDelta - DELTA_SIC)
|
|
else
|
|
bVDeltaS = ( dNewVDelta > dVDelta + dVDeltaTol or dNewVDelta < dVDelta - DELTA_SIC)
|
|
end
|
|
-- reimposto i delta finali in caso di spostamenti richiesti significativi, tenuto conto delle tolleranze
|
|
if bVDeltaS then
|
|
dNewVDelta = dNewVDelta - dVDeltaTol/2
|
|
else
|
|
dNewVDelta = dVDelta
|
|
end
|
|
dNewYDelta = nil
|
|
-- altrimenti errore
|
|
else
|
|
if EgtGetDebugLevel() < 3 then
|
|
EgtOutLog( ' Dist/Min : Back=' .. EgtNumToString( dDistBackEff, 1) .. '/' .. EgtNumToString( MinBackJoin, 1) ..
|
|
' Front=' .. EgtNumToString( dDistFrontEff, 1) .. '/' .. EgtNumToString( MinFrontJoin, 1))
|
|
end
|
|
EgtOutLog( ' Error CLAMP impossible')
|
|
EMC.ERR = 18
|
|
return {}
|
|
end
|
|
|
|
-- salvo valori finali
|
|
EMC.YDELTANEXT = dNewYDelta
|
|
EMC.VDELTANEXT = dNewVDelta
|
|
|
|
local vCmd = {}
|
|
SpecAdjustCarriages( vCmd, dPosT, dYDelta, dVDelta, dNewPosT, dNewYDelta, dNewVDelta, bFixedDelta, bFixedPos)
|
|
|
|
SpecOutputCNT()
|
|
return vCmd
|
|
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function SpecCalcSplit( dLenRaw, dMaxLenLeft)
|
|
local vCmd = {}
|
|
EgtOutLog( ' *[S]', 1)
|
|
local bSplit = ( EMC.VDELTA ~= nil)
|
|
local ParkT = LoadT
|
|
if bSplit then
|
|
if dMaxLenLeft + 100 > LoadT then
|
|
ParkT = dMaxLenLeft + 300
|
|
end
|
|
end
|
|
table.insert( vCmd, { 0, EgtIf( bSplit, 'Split', 'Fall')})
|
|
-- determino i grezzi da agganciare al carrello Y (sono quelli presenti nella fase successiva dispari)
|
|
local nNextOddPhase = GetNextStartOrRestPhase( EMC.PHASE)
|
|
local nRawId = EgtGetFirstRawPart()
|
|
while nRawId do
|
|
if EgtVerifyRawPartPhase( nRawId, nNextOddPhase) then
|
|
table.insert( vCmd, { 31, nRawId, 'Y'})
|
|
end
|
|
nRawId = EgtGetNextRawPart( nRawId)
|
|
end
|
|
-- riporto il carrello Y al carico con il resto della trave
|
|
local dLDelta = EMC.YDELTA - dLenRaw
|
|
table.insert( vCmd, { 1, 'Y', ParkT + dLDelta})
|
|
table.insert( vCmd, { 21, 0, EMC.VDELTA or 0})
|
|
-- imposto subito Y non più attaccato al trave in lavoro
|
|
EMC.YDELTA = nil
|
|
-- salvo posizione carrello Y in disposizione del pezzo dopo split
|
|
local PostDispId = EgtGetPhaseDisposition( EMC.PHASE + 1)
|
|
if PostDispId then
|
|
if not bSplit then
|
|
EgtSetInfo( PostDispId, 'TPOS', ParkT)
|
|
else
|
|
EgtRemoveInfo( PostDispId, 'TPOS')
|
|
EgtSetInfo( PostDispId, 'TPARK', ParkT)
|
|
end
|
|
EgtSetInfo( PostDispId, 'YPOS', ParkT + dLDelta)
|
|
end
|
|
-- salvo posizione grezzo rimasto e posizione carrello Y nella disposizione iniziale del pezzo succ (prossima fase dispari)
|
|
local NextDispId = EgtGetPhaseDisposition( nNextOddPhase)
|
|
if NextDispId then
|
|
EgtSetInfo( NextDispId, 'TPOS', ParkT)
|
|
EgtSetInfo( NextDispId, 'YPOS', ParkT + dLDelta)
|
|
end
|
|
return vCmd
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function SpecCalcSplitRot( dLenRaw)
|
|
local vCmd = {}
|
|
EgtOutLog( ' *[SR]', 1)
|
|
table.insert( vCmd, { 0, 'SplitRot'})
|
|
-- determino i grezzi da agganciare al carrello Y (sono quelli presenti nella fase successiva dispari)
|
|
local vRaw = {}
|
|
local nNextOddPhase = GetNextStartOrRestPhase( EMC.PHASE)
|
|
local nRawId = EgtGetFirstRawPart()
|
|
while nRawId do
|
|
if EgtVerifyRawPartPhase( nRawId, nNextOddPhase) then
|
|
table.insert( vRaw, nRawId)
|
|
end
|
|
nRawId = EgtGetNextRawPart( nRawId)
|
|
end
|
|
for _, nId in ipairs( vRaw) do
|
|
table.insert( vCmd, { 31, nId, 'Y'})
|
|
end
|
|
-- riporto il carrello Y al carico con il resto della trave
|
|
local dLDelta = EMC.YDELTA - dLenRaw
|
|
table.insert( vCmd, { 1, 'Y', LoadT + TurnerOffs + dLDelta})
|
|
table.insert( vCmd, { 21, 0, EMC.VDELTA})
|
|
-- imposto subito Y non più attaccato alla trave in lavoro
|
|
EMC.YDELTA = nil
|
|
-- apro il carrello Y
|
|
table.insert( vCmd, { 11, 0})
|
|
-- sgancio i grezzi dal carrello Y
|
|
for _, nId in ipairs( vRaw) do
|
|
table.insert( vCmd, { 31, nId, ''})
|
|
end
|
|
-- lo porto in parcheggio
|
|
table.insert( vCmd, { 1, 'Y', ParkY})
|
|
-- salvo posizione grezzo rimasto nella disposizione iniziale del pezzo succ (prossima fase dispari)
|
|
local NextDispId = EgtGetPhaseDisposition( nNextOddPhase)
|
|
if NextDispId then
|
|
EgtSetInfo( NextDispId, 'TPOS', LoadT)
|
|
end
|
|
return vCmd
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function SpecCalcUnload()
|
|
local vCmdPre = {}
|
|
EgtOutLog( ' *[U]', 1)
|
|
-- Se pinza Y chiusa , devo effettuare uno scambio
|
|
if EMC.YDELTA then
|
|
-- determino posizione testa trave
|
|
local nLastEntId = EgtGetLastInGroup( EMC.PATHID)
|
|
local vAxes = EmtGetAxesPos( nLastEntId)
|
|
if #vAxes > 0 then EMC.TPOS = vAxes[1] end
|
|
-- imposto quote aggancio per avere solo pinza V
|
|
local dDistFront = EMC.LB - MinOther - EMC.TCING - 10 * GEO.EPS_SMALL
|
|
local dDistBack = 0
|
|
-- effettuo scambio
|
|
vCmdPre = SpecCalcCarriages( dDistFront, dDistBack)
|
|
if EMC.ERR ~= 0 then
|
|
return {}
|
|
end
|
|
-- recupero nuova posizione carrelli
|
|
SpecSetCarrPosFromCmds( vCmdPre)
|
|
EgtOutLog( ' *[U1]', 1)
|
|
end
|
|
local vCmd = {}
|
|
-- Tipo di scarico
|
|
local bStdUl = ( MaxUnloadLen < 1 or EMC.LB - EMC.HOVM < MaxUnloadLen + 1)
|
|
-- Commento
|
|
table.insert( vCmd, { 0, 'Unloading', EgtIf( bStdUl, 'Unloading', 'Manual Unloading')})
|
|
-- risalita testa a Zmax
|
|
local bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
-- Se pinza Y chiusa, la apro
|
|
if EMC.YDELTA then
|
|
table.insert( vCmd, { 11, 0})
|
|
end
|
|
-- Se non supero la lunghezza massima di scarico, sposto il pezzo in posizione di scarico
|
|
if bStdUl then
|
|
local dFinT = UnloadT - EMC.LB
|
|
local dFinV = dFinT + EMC.VDELTA
|
|
table.insert( vCmd, { 2, 'T', dFinT, 'V', dFinV})
|
|
else
|
|
table.insert( vCmd, { 1, 'V', MaxV})
|
|
end
|
|
-- apro la morsa
|
|
table.insert( vCmd, { 12, 0})
|
|
-- riporto il carrello in home
|
|
table.insert( vCmd, { 1, 'V', ParkV})
|
|
|
|
-- eventuale unione tabelle
|
|
if #vCmdPre > 0 then
|
|
vCmd = EgtJoinTables( vCmdPre, vCmd)
|
|
end
|
|
|
|
return vCmd
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function SpecCalcPreRot()
|
|
local vCmdPre = {}
|
|
EgtOutLog( ' *[PR]', 1)
|
|
-- Se pinza V chiusa , devo effettuare uno scambio
|
|
if EMC.VDELTA then
|
|
-- imposto quote aggancio per avere solo pinza Y
|
|
local dDistFront = 0
|
|
local dDistBack = EMC.LB - MinOther - EMC.HOVM + 10 * GEO.EPS_SMALL
|
|
-- effettuo scambio
|
|
vCmdPre = SpecCalcCarriages( dDistFront, dDistBack)
|
|
-- recupero nuova posizione carrelli
|
|
SpecSetCarrPosFromCmds( vCmdPre)
|
|
EgtOutLog( ' *[PR1]', 1)
|
|
end
|
|
-- porto il pezzo alla zona di rotazione con il carro Y
|
|
local vCmd = {}
|
|
-- Commento
|
|
table.insert( vCmd, { 0, 'Pre-Rotation'})
|
|
-- risalita testa a Zmax
|
|
local bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
-- Se pinza V chiusa, la apro
|
|
if EMC.VDELTA then
|
|
table.insert( vCmd, { 12, 0})
|
|
end
|
|
-- riporto la trave al carico
|
|
local RotT = LoadT + TurnerOffs - EMC.HOVM
|
|
table.insert( vCmd, { 2, 'Y', RotT + EMC.YDELTA, 'T', RotT})
|
|
-- apro la morsa
|
|
table.insert( vCmd, { 11, 0})
|
|
-- riporto il carrello in home
|
|
table.insert( vCmd, { 1, 'Y', ParkY})
|
|
|
|
-- eventuale unione tabelle
|
|
if #vCmdPre > 0 then
|
|
vCmd = EgtJoinTables( vCmdPre, vCmd)
|
|
end
|
|
|
|
return vCmd
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function CalcCharStatus( sType, dDelta)
|
|
-- se per carrello Y
|
|
if sType == 'Y' then
|
|
return EgtIf( EMC.LB - dDelta < LenToPress, 1, 2)
|
|
-- altrimenti per carrello V
|
|
else
|
|
return EgtIf( dDelta < LenToPress, 1, 2)
|
|
end
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function SpecAdjustCarriages( vCmd, dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta, bFixedPos)
|
|
local dYPosA, dVPosA, dTPosA
|
|
local dGainOnReclamping = BD.GAIN_RECLAMPING or 1000
|
|
|
|
-- se primo scambio
|
|
local MyMinY = EgtIf( EMC.CNT == 1, MinY + AGG_LOAD, MinY)
|
|
|
|
-- se pinza non in presa, setto offset minimo in base all'altra che sta pinzando
|
|
if not dYDeltaI then
|
|
dYDeltaI = dVDeltaI + ( MyMinY - MaxV)
|
|
end
|
|
if not dVDeltaI then
|
|
dVDeltaI = dYDeltaI - ( MyMinY - MaxV)
|
|
end
|
|
if not dYDeltaF then
|
|
dYDeltaF = dVDeltaF + ( MyMinY - MaxV)
|
|
end
|
|
if not dVDeltaF then
|
|
dVDeltaF = dYDeltaF - ( MyMinY - MaxV)
|
|
end
|
|
|
|
-- se non c'è posizione finale, è lo scarico. Allora setto al minimo della morsa allo scarico.
|
|
if not dTPosF then
|
|
dTPosF = MaxV - dVDeltaF
|
|
end
|
|
|
|
-- se il pinzaggio deve essere invertito forzo entrata nei riposizionamenti
|
|
-- altrimenti posizioni iniziali e finali sono coincidenti dato che la pinza non in presa viene posizionata alla distanza minima a quella in presa
|
|
local bInvertClamping = not EMC.PILGRIMSTEP and EMC.YDELTA ~= EMC.YDELTANEXT and EMC.VDELTA ~= EMC.VDELTANEXT
|
|
-- verifico che le morse non sono in posizione
|
|
if ( abs( dYDeltaF - dYDeltaI) > 10 * GEO.EPS_SMALL or abs( dVDeltaF - dVDeltaI) > 10 * GEO.EPS_SMALL or bInvertClamping) and
|
|
( EMC.YDELTA ~= EMC.YDELTANEXT or EMC.VDELTA ~= EMC.VDELTANEXT) then
|
|
if #vCmd == 0 then
|
|
table.insert( vCmd, { 0, 'Clamp repositioning'})
|
|
-- risalita testa a Zmax
|
|
local bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
end
|
|
|
|
-- ribadisco pinzaggio
|
|
if EMC.YDELTA then
|
|
table.insert( vCmd, { 11, 1})
|
|
end
|
|
if EMC.VDELTA then
|
|
table.insert( vCmd, { 12, 1})
|
|
end
|
|
|
|
-- reset contatore primo riposizionamento con compensazione lettura laser
|
|
EMC.CNT = nil
|
|
|
|
-- ricavo posizione delle morse nelle corse
|
|
local dYPosI = max( MyMinY, dTPosI + dYDeltaI)
|
|
local dVPosI = min( MaxV, dTPosI + dVDeltaI)
|
|
|
|
-- calcolo eventuale massimo riposizionamento con passo del pellegrino.
|
|
-- al primo step si calcolano posizioni attuali delle morse
|
|
-- al secondo step si considerano le corse massime
|
|
local dMaxMovePilgrimStepDoubleClamp, dMaxMovePilgrimStepSingleClampY, dMaxMovePilgrimStepSingleClampV
|
|
if not EMC.PILGRIMSTEP then
|
|
dMaxMovePilgrimStepDoubleClamp = MaxY - dYPosI - MinV + dVPosI
|
|
dMaxMovePilgrimStepSingleClampY = MaxY - dYPosI
|
|
dMaxMovePilgrimStepSingleClampV = - MinV + dVPosI
|
|
else
|
|
dMaxMovePilgrimStepDoubleClamp = MaxY - MyMinY - MinV + MaxV
|
|
dMaxMovePilgrimStepSingleClampY = MaxY - MyMinY
|
|
dMaxMovePilgrimStepSingleClampV = - MinV + MaxV
|
|
end
|
|
|
|
-- calcolo delta spostamento trave
|
|
local dDeltaBeam = dTPosI - dTPosF -- se positivo, la trave si sposta dal carico allo scarico
|
|
local dBeamMove = 0
|
|
|
|
-- PRIMO CASO -> SCAMBIO DIRETTO : V IN POSIZIONE FINALE
|
|
-- V può andare in posizione direttamente se :
|
|
-- * rispetta interasse minimo con Y
|
|
-- * deve effettivamente spostarsi
|
|
-- * non si arriva da gestione passo del pellegrino
|
|
-- * se riesce a raggiungere la posizione con le corse a disposizione per come sono posizionate le morse attualmente
|
|
if ( dYDeltaI - dVDeltaF) + 10 * GEO.EPS_SMALL >= MyMinY - MaxV and
|
|
( abs( dVDeltaF - dVDeltaI) > 10 * GEO.EPS_SMALL or not EMC.VDELTA) and
|
|
not EMC.PILGRIMSTEP and
|
|
( dVDeltaF - dVDeltaI > MinV - dVPosI + MyMinY - dYPosI) then
|
|
table.insert( vCmd, { 0, 'Direct-V-Y'})
|
|
-- se l'altra morsa non era in presa, vado a pinzare il pezzo
|
|
if not EMC.YDELTA then
|
|
dYPosA = MyMinY
|
|
dTPosA = MyMinY - dYDeltaI
|
|
dVPosA = dTPosA + dVDeltaI
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
dTPosI = dTPosA
|
|
dVPosI = dVPosA
|
|
table.insert( vCmd, { 11, 1})
|
|
end
|
|
|
|
-- se spostando morsa diretto esco dalle corse, calcolo di quanto devo muovere la trave (cioè l'altra morsa) come minimo
|
|
-- se non era ingaggiata, vado al suo minimo
|
|
if not EMC.VDELTA then
|
|
dBeamMove = -( MaxV - dVDeltaF - dTPosI)
|
|
-- se spostamento verso lo scarico
|
|
elseif ( dVPosI + ( dVDeltaF - dVDeltaI)) < MinV then
|
|
dBeamMove = dVPosI + ( dVDeltaF - dVDeltaI) - MinV
|
|
-- se spostamento verso il carico
|
|
elseif ( dVPosI + ( dVDeltaF - dVDeltaI)) > MaxV then
|
|
dBeamMove = dVPosI + ( dVDeltaF - dVDeltaI) - MaxV
|
|
-- se è FASTCLAMPING attivo, si suddivide il movimento
|
|
elseif BD.FASTCLAMPING then
|
|
-- se la trave si sposta dal carico verso lo scarico
|
|
if dTPosI - dTPosF > 0 then
|
|
dBeamMove = min( -( dTPosF - dTPosI), -( MyMinY - dYPosI), -( dVDeltaI - dVDeltaF))
|
|
else
|
|
dBeamMove = max( -( dTPosF - dTPosI), -( MyMinY - dYPosI))
|
|
end
|
|
dBeamMove = dBeamMove / 2
|
|
-- altrimenti la trave resta ferma e la pinza 1 va in presa (si limita lo scivolamento del pezzo)
|
|
else
|
|
;
|
|
end
|
|
|
|
-- se serve un grande spostamento e si sta pinzando poco. Y trascinatore pinza poco e V si sposta per prima per recuperare
|
|
if not BD.FASTCLAMPING and abs( dBeamMove) > 2000 and dDeltaBeam < 0 and EMC.LB - dYDeltaI < max( 550, (MinJoin * 2)) then
|
|
table.insert( vCmd, { 12, 0})
|
|
dVPosA = dVPosI - dGainOnReclamping
|
|
-- sposto il carrello V di 1000mm
|
|
table.insert( vCmd, { 1, 'V', dVPosA})
|
|
table.insert( vCmd, { 12, 1})
|
|
-- apro morsa Y e recupero i 1000mm
|
|
table.insert( vCmd, { 11, 0})
|
|
dVPosA = dVPosI
|
|
dYPosA = dYPosI
|
|
dTPosA = dTPosI + dGainOnReclamping
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
dTPosI = dTPosA
|
|
dVPosI = dVPosA
|
|
dVDeltaI = dVDeltaI - dGainOnReclamping
|
|
dYDeltaI = dYDeltaI - dGainOnReclamping
|
|
table.insert( vCmd, { 11, 1})
|
|
dBeamMove = dBeamMove + dGainOnReclamping
|
|
end
|
|
|
|
table.insert( vCmd, { 12, 0})
|
|
|
|
dTPosA = dTPosI - dBeamMove
|
|
dYPosA = dTPosA + dYDeltaI
|
|
dVPosA = dTPosA + dVDeltaF
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- la morsa allo scarico è adesso in posizione
|
|
dVDeltaI = dVDeltaF
|
|
dTPosI = dTPosA
|
|
|
|
-- blocco la morsa
|
|
table.insert( vCmd, { 12, CalcCharStatus( 'V', dVDeltaF)})
|
|
|
|
-- imposto i nuovi parametri di aggancio
|
|
if EMC.YDELTANEXT and EMC.VDELTANEXT then
|
|
-- porto assi alla loro posizione finale
|
|
if abs( dYDeltaF - dYDeltaI) > 10 * GEO.EPS_SMALL then
|
|
table.insert( vCmd, { 11, 0})
|
|
dTPosA = dTPosF
|
|
dYPosA = dTPosF + dYDeltaF
|
|
dVPosA = dTPosF + dVDeltaF
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
table.insert( vCmd, { 11, CalcCharStatus( 'Y', dYDeltaF)})
|
|
-- la morsa al carico è adesso in posizione
|
|
dYDeltaI = dYDeltaF
|
|
end
|
|
table.insert( vCmd, { 21, dYDeltaF, dVDeltaF})
|
|
elseif EMC.YDELTANEXT then
|
|
if abs( dYDeltaF - dYDeltaI) > 10 * GEO.EPS_SMALL then
|
|
table.insert( vCmd, { 11, 0})
|
|
dYPosA = max( MyMinY, dTPosI + dYDeltaF)
|
|
dTPosA = dYPosA - dYDeltaF
|
|
dVPosA = dTPosA + dVDeltaF
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
table.insert( vCmd, { 11, CalcCharStatus( 'Y', dYDeltaF)})
|
|
-- la morsa al carico è adesso in posizione
|
|
dYDeltaI = dYDeltaF
|
|
end
|
|
table.insert( vCmd, { 12, 0})
|
|
-- sposto il carrello V in parcheggio
|
|
table.insert( vCmd, { 1, 'V', ParkV})
|
|
table.insert( vCmd, { 21, dYDeltaF, 0})
|
|
else
|
|
table.insert( vCmd, { 11, 0})
|
|
table.insert( vCmd, { 1, 'Y', ParkY})
|
|
table.insert( vCmd, { 21, 0, dVDeltaF})
|
|
end
|
|
|
|
-- movimento terminato
|
|
dTPosI, dYDeltaI, dVDeltaI = dTPosF, dYDeltaF, dVDeltaF
|
|
EMC.YDELTA = EMC.YDELTANEXT
|
|
EMC.VDELTA = EMC.VDELTANEXT
|
|
|
|
-- SECONDO CASO -> SCAMBIO DIRETTO : Y IN POSIZIONE FINALE
|
|
-- Y può andare in posizione direttamente se :
|
|
-- * rispetta interasse minimo con Y
|
|
-- * deve effettivamente spostarsi
|
|
-- * non si arriva da gestione passo del pellegrino
|
|
-- * se riesce a raggiungere la posizione con le corse a disposizione per come sono posizionate le morse attualmente
|
|
elseif ( dYDeltaF - dVDeltaI) + 10 * GEO.EPS_SMALL >= MyMinY - MaxV and
|
|
( abs( dYDeltaF - dYDeltaI) > 10 * GEO.EPS_SMALL or not EMC.YDELTA) and
|
|
not EMC.PILGRIMSTEP and
|
|
( dYDeltaF - dYDeltaI < MaxY - dYPosI + MaxV - dVPosI) then
|
|
table.insert( vCmd, { 0, 'Direct-Y-V'})
|
|
-- se l'altra morsa non era in presa, vado a pinzare il pezzo
|
|
if not EMC.VDELTA then
|
|
dVPosA = MaxV
|
|
dTPosA = MaxV - dVDeltaI
|
|
dYPosA = dTPosA + dYDeltaI
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
dTPosI = dTPosA
|
|
dYPosI = dYPosA
|
|
table.insert( vCmd, { 12, 1})
|
|
else
|
|
-- compenso delta errore misurazione laser
|
|
dVDeltaI = dVDeltaI - EgtIf( EMC.CNT == 1, AGG_LOAD, 0)
|
|
end
|
|
|
|
-- se spostando morsa diretto esco dalle corse, calcolo di quanto devo muovere la trave (cioè l'altra morsa) come minimo
|
|
-- se non era ingaggiata, vado al suo minimo
|
|
if not EMC.YDELTA then
|
|
dBeamMove = - ( dYDeltaF - MyMinY + dTPosI)
|
|
-- se spostamento verso lo scarico
|
|
elseif ( dYPosI + ( dYDeltaF - dYDeltaI)) < MyMinY then
|
|
dBeamMove = -( dYPosI + ( dYDeltaF - dYDeltaI) - MyMinY)
|
|
-- se spostamento verso il carico
|
|
elseif ( dYPosI + ( dYDeltaF - dYDeltaI)) > MaxY then
|
|
dBeamMove = -( dYPosI + ( dYDeltaF - dYDeltaI) - MaxY)
|
|
-- se è FASTCLAMPING attivo, si suddivide il movimento
|
|
elseif BD.FASTCLAMPING then
|
|
-- se la trave si sposta dal carico verso lo scarico
|
|
if dTPosI - dTPosF > 0 then
|
|
dBeamMove = max( ( dTPosF - dTPosI), ( MinV - dVPosI), ( dYDeltaI - dYDeltaF))
|
|
else
|
|
dBeamMove = min( ( dTPosF - dTPosI), ( MaxV - dVPosI))
|
|
end
|
|
dBeamMove = dBeamMove / 2
|
|
-- altrimenti la trave resta ferma e la pinza 1 va in presa (si limita lo scivolamento del pezzo)
|
|
else
|
|
;
|
|
end
|
|
|
|
-- se serve un grande spostamento e si sta pinzando poco. V trascinatore pinza poco e Y si sposta per prima per recuperare
|
|
if not BD.FASTCLAMPING and abs( dBeamMove) > 2000 and dDeltaBeam > 0 and dVDeltaI < max( 550, (MinJoin * 2)) then
|
|
table.insert( vCmd, { 11, 0})
|
|
dYPosA = dYPosI + dGainOnReclamping
|
|
-- sposto il carrello V di 1000mm
|
|
table.insert( vCmd, { 1, 'Y', dYPosA})
|
|
table.insert( vCmd, { 11, 1})
|
|
-- apro morsa Y e recupero i 1000mm
|
|
table.insert( vCmd, { 12, 0})
|
|
dVPosA = dVPosI
|
|
dYPosA = dYPosI
|
|
dTPosA = dTPosI - dGainOnReclamping
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
dTPosI = dTPosA
|
|
dVPosI = dVPosA
|
|
dVDeltaI = dVDeltaI + dGainOnReclamping
|
|
dYDeltaI = dYDeltaI + dGainOnReclamping
|
|
table.insert( vCmd, { 12, 1})
|
|
dBeamMove = dBeamMove - dGainOnReclamping
|
|
end
|
|
|
|
table.insert( vCmd, { 11, 0})
|
|
|
|
dTPosA = dTPosI + dBeamMove
|
|
dYPosA = dTPosA + dYDeltaF
|
|
dVPosA = dTPosA + dVDeltaI
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- la morsa allo scarico è adesso in posizione
|
|
dYDeltaI = dYDeltaF
|
|
dTPosI = dTPosA
|
|
|
|
-- blocco la morsa
|
|
table.insert( vCmd, { 11, CalcCharStatus( 'Y', dYDeltaF)})
|
|
|
|
-- imposto i nuovi parametri di aggancio
|
|
if EMC.YDELTANEXT and EMC.VDELTANEXT then
|
|
if abs( dVDeltaF - dVDeltaI) > 10 * GEO.EPS_SMALL then
|
|
table.insert( vCmd, { 12, 0})
|
|
dTPosA = dTPosF
|
|
dYPosA = dTPosF + dYDeltaF
|
|
dVPosA = dTPosF + dVDeltaF
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
table.insert( vCmd, { 12, CalcCharStatus( 'V', dVDeltaF)})
|
|
-- la morsa al carico è adesso in posizione
|
|
dVDeltaI = dVDeltaF
|
|
end
|
|
table.insert( vCmd, { 21, dYDeltaF, dVDeltaF})
|
|
elseif EMC.VDELTANEXT then
|
|
if abs( dVDeltaF - dVDeltaI) > 10 * GEO.EPS_SMALL then
|
|
table.insert( vCmd, { 12, 0})
|
|
dVPosA = min( MaxV, dTPosI + dVDeltaF)
|
|
dTPosA = dVPosA - dVDeltaF
|
|
dYPosA = dTPosA + dYDeltaF
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
table.insert( vCmd, { 12, CalcCharStatus( 'V', dVDeltaF)})
|
|
-- la morsa al carico è adesso in posizione
|
|
dVDeltaI = dVDeltaF
|
|
end
|
|
table.insert( vCmd, { 11, 0})
|
|
-- sposto il carrello Y in parcheggio
|
|
table.insert( vCmd, { 1, 'Y', ParkY})
|
|
table.insert( vCmd, { 21, 0, dVDeltaF})
|
|
else
|
|
table.insert( vCmd, { 12, 0})
|
|
-- sposto il carrello V in parcheggio
|
|
table.insert( vCmd, { 1, 'V', ParkV})
|
|
table.insert( vCmd, { 21, dYDeltaF, 0})
|
|
end
|
|
-- movimento terminato
|
|
dTPosI, dYDeltaI, dVDeltaI = dTPosF, dYDeltaF, dVDeltaF
|
|
EMC.YDELTA = EMC.YDELTANEXT
|
|
EMC.VDELTA = EMC.VDELTANEXT
|
|
|
|
-- TERZO CASO -> PASSO DEL PELLEGRINO
|
|
-- non è possibile fare scambio diretto.
|
|
-- le morse vengono compattate e allargate sfruttando tutta la corsa X, massimizzando spostamento trave
|
|
else
|
|
EMC.PILGRIMSTEP = false
|
|
local bUpdateValues = false
|
|
table.insert( vCmd, { 0, 'Pilgrim Cycle'})
|
|
-- compatto al centro
|
|
if not EMC.YDELTA then
|
|
table.insert( vCmd, { 0, 'Compact Y'})
|
|
table.insert( vCmd, { 11, 0})
|
|
dTPosA = MaxV - dVDeltaI
|
|
dVPosA = MaxV
|
|
dYPosA = MyMinY
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
table.insert( vCmd, { 11, 1})
|
|
dYDeltaI = dVDeltaI - MaxV + MyMinY
|
|
EMC.YDELTA = dYDeltaI
|
|
bUpdateValues = true
|
|
end
|
|
if not EMC.VDELTA then
|
|
table.insert( vCmd, { 0, 'Compact V'})
|
|
table.insert( vCmd, { 12, 0})
|
|
dTPosA = MyMinY - dYDeltaI
|
|
dVPosA = MaxV
|
|
dYPosA = MyMinY
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
table.insert( vCmd, { 12, 1})
|
|
dVDeltaI = dYDeltaI + MaxV - MyMinY
|
|
EMC.VDELTA = dVDeltaI
|
|
bUpdateValues = true
|
|
end
|
|
|
|
-- se si sono compattate le morse, bisogna aggiornare le posizioni e i massimi recuperi
|
|
if bUpdateValues then
|
|
dTPosI = dTPosA
|
|
dYPosI = dYPosA
|
|
dVPosI = dVPosA
|
|
dMaxMovePilgrimStepDoubleClamp = MaxY - MyMinY - MinV + MaxV
|
|
dMaxMovePilgrimStepSingleClampY = MaxY - MyMinY
|
|
dMaxMovePilgrimStepSingleClampV = - MinV + MaxV
|
|
end
|
|
|
|
-- aggiorno delta mancante
|
|
dDeltaBeam = dTPosI - dTPosF
|
|
|
|
-- a questo punto entrambe le pinze sono in presa sul pezzo.
|
|
-- trave si muove dal carico allo scarico
|
|
-- Il trascinatore si decide in base alla direzione di movimento trave
|
|
-- trascino con morsa allo scarico. Obiettivo mandare Y in posizione per prima
|
|
if dDeltaBeam > 0 then
|
|
-- se serve un grande spostamento e si sta pinzando poco. Y trascinatore pinza poco e V si sposta per prima per recuperare
|
|
if not BD.FASTCLAMPING and abs( dDeltaBeam) > 2000 and dVDeltaI < max( 550, (MinJoin * 2)) then
|
|
table.insert( vCmd, { 11, 0})
|
|
dYPosA = dYPosI + dGainOnReclamping
|
|
-- sposto il carrello V di 1000mm
|
|
table.insert( vCmd, { 1, 'Y', dYPosA})
|
|
table.insert( vCmd, { 11, 1})
|
|
-- apro morsa Y e recupero i 1000mm
|
|
table.insert( vCmd, { 12, 0})
|
|
dVPosA = dVPosI
|
|
dYPosA = dYPosI
|
|
dTPosA = dTPosI - dGainOnReclamping
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
dTPosI = dTPosA
|
|
dVPosI = dVPosA
|
|
dVDeltaI = dVDeltaI + dGainOnReclamping
|
|
dYDeltaI = dYDeltaI + dGainOnReclamping
|
|
table.insert( vCmd, { 12, 1})
|
|
end
|
|
|
|
-- apro morsa al carico
|
|
table.insert( vCmd, { 11, 0})
|
|
-- devo recuperare più delle corse disponibili di entrambi i carrelli
|
|
if abs( dYDeltaI - dYDeltaF) > dMaxMovePilgrimStepDoubleClamp then
|
|
dYPosA = MaxY
|
|
dVPosA = MinV
|
|
dTPosA = dTPosI - dVPosI + MinV
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- nuova posizione della morsa al carico dopo riposizionamento
|
|
dYDeltaI = MaxY - dTPosA
|
|
dTPosI = dTPosA
|
|
EMC.YDELTA = dYDeltaI
|
|
EMC.VDELTA = nil
|
|
EMC.PILGRIMSTEP = true
|
|
-- mi serve riposizonare di nuovo, quindi richiamo funzione in modo ricorsivo
|
|
SpecAdjustCarriages( vCmd, dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta, bFixedPos)
|
|
-- recupero una corsa massima e l'altra solo di quello che serve
|
|
elseif abs( dYDeltaI - dYDeltaF) > dMaxMovePilgrimStepSingleClampV then
|
|
-- suddivido il movimento su entrambe le morse
|
|
local dTotMove = ( ( dYDeltaF + dVDeltaF) / 2 + ( dYDeltaI + dVDeltaI) / 2) / 2
|
|
-- morsa Y arriva in posizione finale
|
|
dVPosA = EgtClamp( MaxV - dTotMove, MaxV, MinV)
|
|
dTPosA = dTPosI - dVPosI + dVPosA
|
|
dYPosA = dYDeltaF + dTPosA
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
table.insert( vCmd, { 11, CalcCharStatus( 'Y', dYDeltaF)})
|
|
table.insert( vCmd, { 12, 0})
|
|
dYDeltaI = dYDeltaF
|
|
dTPosI = dTPosA
|
|
EMC.YDELTA = dYDeltaF
|
|
EMC.VDELTA = nil
|
|
-- mi serve riposizonare di nuovo, quindi richiamo funzione in modo ricorsivo
|
|
SpecAdjustCarriages( vCmd, dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta, bFixedPos)
|
|
-- non serve recuperare al massimo della corsa, suddivido movimento in due
|
|
else
|
|
-- suddivido il movimento su entrambe le morse
|
|
local dTotMove = ( dYDeltaF - dYDeltaI) / 2
|
|
|
|
if abs( dTotMove) > 10 * GEO.EPS_SMALL then
|
|
dYPosA = dTPosI + dYDeltaI + dTotMove
|
|
dVPosA = dTPosI + dVDeltaI - dTotMove
|
|
dTPosA = dTPosI - dTotMove
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
table.insert( vCmd, { 11, CalcCharStatus( 'Y', dYDeltaF)})
|
|
table.insert( vCmd, { 12, 0})
|
|
|
|
-- aggiorno nuova posizione della morsa al carico dopo riposizionamento
|
|
dYDeltaI = dYDeltaF ;
|
|
|
|
if not EMC.VDELTANEXT then
|
|
dTPosA = dTPosF
|
|
dYPosA = dTPosF + dYDeltaF
|
|
dVPosA = ParkV
|
|
elseif EMC.YDELTANEXT then
|
|
dTPosA = dTPosF
|
|
dYPosA = dTPosF + dYDeltaF
|
|
dVPosA = dTPosF + dVDeltaF
|
|
else
|
|
dYPosA = MyMinY
|
|
dTPosA = MyMinY - dYDeltaF
|
|
dVPosA = dTPosA + dVDeltaF
|
|
end
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
dTPosI, dYDeltaI, dVDeltaI = dTPosF, dYDeltaF, dVDeltaF
|
|
EMC.YDELTA = EMC.YDELTANEXT
|
|
EMC.VDELTA = EMC.VDELTANEXT
|
|
end
|
|
|
|
-- imposto i nuovi parametri di aggancio
|
|
if EMC.YDELTANEXT and EMC.VDELTANEXT then
|
|
table.insert( vCmd, { 12, CalcCharStatus( 'V', dVDeltaF)})
|
|
table.insert( vCmd, { 21, dYDeltaF, dVDeltaF})
|
|
elseif EMC.YDELTANEXT then
|
|
table.insert( vCmd, { 21, dYDeltaF, 0})
|
|
else
|
|
table.insert( vCmd, { 12, CalcCharStatus( 'V', dVDeltaF)})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- sposto il carrello Y in parcheggio
|
|
table.insert( vCmd, { 1, 'Y', ParkY})
|
|
table.insert( vCmd, { 21, 0, dVDeltaF})
|
|
end
|
|
end
|
|
-- trave si muove dallo scarico verso il carico. Obiettivo mandare V in posizione per prima
|
|
else
|
|
-- se serve un grande spostamento e si sta pinzando poco. Y trascinatore pinza poco e V si sposta per prima per recuperare
|
|
if not BD.FASTCLAMPING and abs( dDeltaBeam) > 2000 and EMC.LB - dYDeltaI < max( 550, (MinJoin * 2)) then
|
|
table.insert( vCmd, { 12, 0})
|
|
dVPosA = dVPosI - dGainOnReclamping
|
|
-- sposto il carrello V di 1000mm
|
|
table.insert( vCmd, { 1, 'V', dVPosA})
|
|
table.insert( vCmd, { 12, 1})
|
|
-- apro morsa Y e recupero i 1000mm
|
|
table.insert( vCmd, { 11, 0})
|
|
dVPosA = dVPosI
|
|
dYPosA = dYPosI
|
|
dTPosA = dTPosI + dGainOnReclamping
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
dTPosI = dTPosA
|
|
dVPosI = dVPosA
|
|
dVDeltaI = dVDeltaI - dGainOnReclamping
|
|
dYDeltaI = dYDeltaI - dGainOnReclamping
|
|
table.insert( vCmd, { 11, 1})
|
|
end
|
|
|
|
-- apro morsa allo scarico
|
|
table.insert( vCmd, { 12, 0})
|
|
-- devo recuperare più del doppio delle corse
|
|
local dMaxMoveFirst
|
|
if abs( dVDeltaI - dVDeltaF) > dMaxMovePilgrimStepDoubleClamp then
|
|
dYPosA = MaxY
|
|
dVPosA = MinV
|
|
dTPosA = dTPosI + ( MaxY - dYPosI)
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- nuova posizione della morsa allo scarico dopo riposizionamento
|
|
dVDeltaI = MinV - dTPosA
|
|
dTPosI = dTPosA
|
|
EMC.YDELTA = nil
|
|
EMC.VDELTA = dVDeltaI
|
|
EMC.PILGRIMSTEP = true
|
|
-- mi serve riposizonare di nuovo, quindi richiamo funzione in modo ricorsivo
|
|
SpecAdjustCarriages( vCmd, dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta, bFixedPos)
|
|
-- recupero una corsa massima e l'altra solo di quello che serve
|
|
elseif abs( dVDeltaI - dVDeltaF) > dMaxMovePilgrimStepSingleClampY then
|
|
-- suddivido il movimento su entrambe le morse
|
|
local dTotMove = ( ( dYDeltaF + dVDeltaF) / 2 + ( dYDeltaI + dVDeltaI) / 2) / 2
|
|
-- morsa V arriva in posizione finale
|
|
dYPosA = EgtClamp( MinY + dTotMove, MinY, MaxY)
|
|
dTPosA = dTPosI - dYPosI + dYPosA
|
|
dVPosA = dVDeltaF + dTPosA
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
table.insert( vCmd, { 12, CalcCharStatus( 'V', dVDeltaF)})
|
|
table.insert( vCmd, { 11, 0})
|
|
dVDeltaI = dVDeltaF
|
|
dTPosI = dTPosA
|
|
EMC.YDELTA = nil
|
|
EMC.VDELTA = dVDeltaF
|
|
-- mi serve riposizonare di nuovo, quindi richiamo funzione in modo ricorsivo
|
|
SpecAdjustCarriages( vCmd, dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta, bFixedPos)
|
|
-- non serve recuperare al massimo della corsa, suddivido movimento in due
|
|
else
|
|
-- suddivido il movimento su entrambe le morse
|
|
local dTotMove = ( dVDeltaI - dVDeltaF) / 2
|
|
|
|
if abs( dTotMove) > 10 * GEO.EPS_SMALL then
|
|
dYPosA = dTPosI + dYDeltaI + dTotMove
|
|
dVPosA = dTPosI + dVDeltaI - dTotMove
|
|
dTPosA = dTPosI + dTotMove
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
table.insert( vCmd, { 12, CalcCharStatus( 'V', dVDeltaF)})
|
|
table.insert( vCmd, { 11, 0})
|
|
|
|
-- aggiorno nuova posizione della morsa allo scarico dopo riposizionamento
|
|
dVDeltaI = dVDeltaF ;
|
|
|
|
if not EMC.YDELTANEXT then
|
|
dTPosA = dTPosF
|
|
dVPosA = dTPosF + dVDeltaF
|
|
dYPosA = ParkY
|
|
elseif EMC.VDELTANEXT then
|
|
dTPosA = dTPosF
|
|
dYPosA = dTPosF + dYDeltaF
|
|
dVPosA = dTPosF + dVDeltaF
|
|
else
|
|
dVPosA = MaxV
|
|
dTPosA = MaxV - dVDeltaF
|
|
dYPosA = dTPosA + dYDeltaF
|
|
end
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
dTPosI, dYDeltaI, dVDeltaI = dTPosF, dYDeltaF, dVDeltaF
|
|
EMC.YDELTA = EMC.YDELTANEXT
|
|
EMC.VDELTA = EMC.VDELTANEXT
|
|
end
|
|
|
|
-- imposto i nuovi parametri di aggancio
|
|
if EMC.YDELTANEXT and EMC.VDELTANEXT then
|
|
table.insert( vCmd, { 11, CalcCharStatus( 'Y', dYDeltaF)})
|
|
table.insert( vCmd, { 21, dYDeltaF, dVDeltaF})
|
|
elseif EMC.YDELTANEXT then
|
|
table.insert( vCmd, { 11, CalcCharStatus( 'Y', dYDeltaF)})
|
|
-- sposto il carrello Y in parcheggio
|
|
table.insert( vCmd, { 12, 0})
|
|
table.insert( vCmd, { 1, 'V', ParkV})
|
|
table.insert( vCmd, { 21, dYDeltaF, 0})
|
|
else
|
|
table.insert( vCmd, { 21, 0, dVDeltaF})
|
|
end
|
|
end
|
|
end
|
|
end
|
|
end
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function SpecOutputCNT()
|
|
if EMC.CNT == 1 then
|
|
EgtSetInfo( EMC.PATHID, 'CNT', 1)
|
|
else
|
|
EgtRemoveInfo( EMC.PATHID, 'CNT')
|
|
end
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function SpecOutputCmds( vCmd, bEnd)
|
|
|
|
local sRoot = EgtIf( not bEnd, 'AS', 'AE')
|
|
|
|
-- Registro il numero di comandi
|
|
if #vCmd > 0 then
|
|
EgtSetInfo( EMC.PATHID, sRoot..'#', #vCmd)
|
|
else
|
|
EgtRemoveInfo( EMC.PATHID, sRoot..'#')
|
|
end
|
|
-- Registro i comandi
|
|
for i = 1, #vCmd do
|
|
local Cmd = vCmd[i]
|
|
local sKey = sRoot..tostring( i)
|
|
-- commento
|
|
if Cmd[1] == 0 then
|
|
local sInfo = '0,'..Cmd[2]
|
|
if Cmd[3] then sInfo = sInfo..','..Cmd[3] end
|
|
EgtSetInfo( EMC.PATHID, sKey, sInfo)
|
|
-- movimento di 1 asse
|
|
elseif Cmd[1] == 1 then
|
|
local sInfo = '1,'..Cmd[2]..','..EgtNumToString( Cmd[3],3)
|
|
EgtSetInfo( EMC.PATHID, sKey, sInfo)
|
|
-- movimento di 2 assi
|
|
elseif Cmd[1] == 2 then
|
|
local sInfo = '2,'..Cmd[2]..','..EgtNumToString( Cmd[3],3)..','..Cmd[4]..','..EgtNumToString( Cmd[5],3)
|
|
EgtSetInfo( EMC.PATHID, sKey, sInfo)
|
|
-- movimento di 3 assi
|
|
elseif Cmd[1] == 3 then
|
|
local sInfo = '3,'..Cmd[2]..','..EgtNumToString( Cmd[3],3)..','..Cmd[4]..','..EgtNumToString( Cmd[5],3)..','..
|
|
Cmd[6]..','..EgtNumToString( Cmd[7],3)
|
|
EgtSetInfo( EMC.PATHID, sKey, sInfo)
|
|
-- apertura/chiusura morsa Y
|
|
elseif Cmd[1] == 11 then
|
|
local sInfo = '11,'..EgtNumToString( Cmd[2],0)
|
|
EgtSetInfo( EMC.PATHID, sKey, sInfo)
|
|
if Cmd[2] == 0 then EMC.YDELTA = nil end
|
|
-- apertura/chiusura morsa V
|
|
elseif Cmd[1] == 12 then
|
|
local sInfo = '12,'..EgtNumToString( Cmd[2],0)
|
|
EgtSetInfo( EMC.PATHID, sKey, sInfo)
|
|
if Cmd[2] == 0 then EMC.VDELTA = nil end
|
|
-- impostazione nuovo stato dei carrelli
|
|
elseif Cmd[1] == 21 then
|
|
local sInfo = '21,'..EgtNumToString( Cmd[2],3)..','..EgtNumToString( Cmd[3],3)
|
|
EgtSetInfo( EMC.PATHID, sKey, sInfo)
|
|
if Cmd[2] > 0 and Cmd[3] > 0 then
|
|
EMC.YDELTA = Cmd[2]
|
|
EMC.VDELTA = Cmd[3]
|
|
elseif Cmd[2] > 0 then
|
|
EMC.YDELTA = Cmd[2]
|
|
EMC.VDELTA = nil
|
|
elseif Cmd[3] > 0 then
|
|
EMC.YDELTA = nil
|
|
EMC.VDELTA = Cmd[3]
|
|
end
|
|
-- aggancio grezzo a carrello
|
|
elseif Cmd[1] == 31 then
|
|
local sInfo = '31,'..EgtNumToString( Cmd[2],0)..','..Cmd[3]
|
|
EgtSetInfo( EMC.PATHID, sKey, sInfo)
|
|
end
|
|
end
|
|
|
|
-- Salvo i nuovi delta dei carrelli
|
|
if EMC.YDELTA then
|
|
EgtSetInfo( EMC.PATHID, 'YDELTA', EMC.YDELTA)
|
|
else
|
|
EgtRemoveInfo( EMC.PATHID, 'YDELTA')
|
|
end
|
|
if EMC.VDELTA then
|
|
EgtSetInfo( EMC.PATHID, 'VDELTA', EMC.VDELTA)
|
|
else
|
|
EgtRemoveInfo( EMC.PATHID, 'VDELTA')
|
|
end
|
|
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function SpecSetCarrPosFromCmds( vCmd)
|
|
-- recupero nuova posizione carrelli
|
|
for i = 1, #vCmd do
|
|
local Cmd = vCmd[i]
|
|
if Cmd[1] == 21 then
|
|
if Cmd[2] > 0 and Cmd[3] > 0 then
|
|
EMC.YDELTA = Cmd[2]
|
|
EMC.VDELTA = Cmd[3]
|
|
elseif Cmd[2] > 0 then
|
|
EMC.YDELTA = Cmd[2]
|
|
EMC.VDELTA = nil
|
|
elseif Cmd[3] > 0 then
|
|
EMC.YDELTA = nil
|
|
EMC.VDELTA = Cmd[3]
|
|
end
|
|
end
|
|
end
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function SpecTestOnlyRemarkInCmds( vCmd)
|
|
if not vCmd then return true end
|
|
-- verifico se nella lista dei comandi ci sono solo commenti
|
|
for i = 1, #vCmd do
|
|
if vCmd[i][1] ~= 0 then
|
|
return false
|
|
end
|
|
end
|
|
return true
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
--XXXXX XXXXX XXXXX --- TO REMOVE --- XXXXX XXXXX XXXXX XXXXX--
|
|
---------------------------------------------------------------------
|
|
---------------------------------------------------------------------
|
|
function SpecCalcCarriagesOLD( dDistFront, dDistBack, bFixedDelta, bFixedPos)
|
|
|
|
local MinFrontJoin = MinJoin + EMC.HCING + EMC.HOVM
|
|
local MinBackJoin = MinJoin + EgtIf( IsMid2Phase( EMC.PHASE) or IsEnd2Phase( EMC.PHASE), EMC.TCING, 0)
|
|
local MyMinOther = MinOther + EgtIf( EMC.CNT == 1, AGG_LOAD, 0)
|
|
|
|
local dDistFrontEff = min( dDistFront, EMC.LB - MyMinOther - EMC.TCING)
|
|
if bFixedDelta and IsMid2Phase( EMC.PHASE + 1) then
|
|
dDistFrontEff = min( dDistFrontEff, EMC.LR - MinOther - EMC.TCING)
|
|
end
|
|
local dDistBackEff = min( dDistBack, EMC.LB - MyMinOther - EMC.HCING - EMC.HOVM)
|
|
|
|
EgtOutLog( ' Dist/Min : Back=' .. EgtNumToString( dDistBackEff, 1) .. '/' .. EgtNumToString( MinBackJoin, 1) ..
|
|
' Front=' .. EgtNumToString( dDistFrontEff, 1) .. '/' .. EgtNumToString( MinFrontJoin, 1) ..
|
|
' Fixed : Delta=' .. EgtIf( bFixedDelta, 'T', 'F') .. ' Pos=' .. EgtIf( bFixedPos, 'T', 'F'), 3)
|
|
|
|
-- [A] se posso mettere solo carrello Y
|
|
if dDistFrontEff < MinFrontJoin and dDistBackEff > MinBackJoin - GEO.EPS_SMALL then
|
|
|
|
local dPosT = EMC.TPOS
|
|
local dYDelta = EMC.YDELTA
|
|
local dVDelta = EMC.VDELTA
|
|
|
|
local dNewPosT = nil
|
|
local dNewYDelta = EMC.LB - dDistBackEff
|
|
local dNewVDelta = nil
|
|
|
|
return SpecAdjustCarriagesOLD( dPosT, dYDelta, dVDelta, dNewPosT, dNewYDelta, dNewVDelta, bFixedDelta, bFixedPos)
|
|
|
|
-- [B] se altrimenti posso mettere entrambi i carrelli Y e V
|
|
elseif dDistBackEff > MinBackJoin - GEO.EPS_SMALL and
|
|
dDistFrontEff > MinFrontJoin - GEO.EPS_SMALL then
|
|
|
|
local dPosT = EMC.TPOS
|
|
local dYDelta = EMC.YDELTA
|
|
local dVDelta = EMC.VDELTA
|
|
|
|
local dNewPosT = nil
|
|
local dNewYDelta = EMC.LB - dDistBackEff
|
|
local dNewVDelta = dDistFrontEff
|
|
|
|
return SpecAdjustCarriagesOLD( dPosT, dYDelta, dVDelta, dNewPosT, dNewYDelta, dNewVDelta, bFixedDelta, bFixedPos)
|
|
|
|
-- [C] se altrimenti posso mettere solo carrello V
|
|
elseif dDistBackEff < MinBackJoin and dDistFrontEff > MinFrontJoin - GEO.EPS_SMALL then
|
|
|
|
local dPosT = EMC.TPOS
|
|
local dYDelta = EMC.YDELTA
|
|
local dVDelta = EMC.VDELTA
|
|
|
|
local dNewPosT = nil
|
|
local dNewYDelta = nil
|
|
local dNewVDelta = dDistFrontEff
|
|
|
|
return SpecAdjustCarriagesOLD( dPosT, dYDelta, dVDelta, dNewPosT, dNewYDelta, dNewVDelta, bFixedDelta, bFixedPos)
|
|
|
|
-- altrimenti errore
|
|
else
|
|
if EgtGetDebugLevel() < 3 then
|
|
EgtOutLog( ' Dist/Min : Back=' .. EgtNumToString( dDistBackEff, 1) .. '/' .. EgtNumToString( MinBackJoin, 1) ..
|
|
' Front=' .. EgtNumToString( dDistFrontEff, 1) .. '/' .. EgtNumToString( MinFrontJoin, 1))
|
|
end
|
|
EgtOutLog( ' Error CLAMP impossible')
|
|
EMC.ERR = 18
|
|
return {}
|
|
end
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
function SpecAdjustCarriagesOLD( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta, bFixedPos)
|
|
-- [A] richiesto solo carrello Y
|
|
if dYDeltaF and ( not dVDeltaF) then
|
|
-- [A1] rimango sul carrello Y
|
|
if dYDeltaI and ( not dVDeltaI) then
|
|
return SpecAdjustCarrA1( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF)
|
|
-- [A2] torno da entrambi
|
|
elseif dYDeltaI and dVDeltaI then
|
|
return SpecAdjustCarrA2( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF)
|
|
-- [A3] torno da carrello V
|
|
elseif dVDeltaI and (not dYDeltaI) then
|
|
return SpecAdjustCarrA3( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF)
|
|
else
|
|
EgtOutLog( ' Error CLAMP NULL-> Y impossibile')
|
|
error( 'Error CLAMP NULL-> Y impossibile')
|
|
end
|
|
-- (B) richiesti entrambi i carrelli
|
|
elseif dYDeltaF and dVDeltaF then
|
|
-- [B1] passo da carrello Y a entrambi
|
|
if dYDeltaI and ( not dVDeltaI) then
|
|
return SpecAdjustCarrB1( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta)
|
|
-- [B2] continuo con entrambi i carrelli
|
|
elseif dYDeltaI and dVDeltaI then
|
|
return SpecAdjustCarrB2( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta)
|
|
-- [B3] passo da carrello V a entrambi
|
|
elseif dVDeltaI and (not dYDeltaI) then
|
|
return SpecAdjustCarrB3( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta)
|
|
else
|
|
EgtOutLog( ' Error CLAMP NULL-> Y+V impossibile')
|
|
error( 'Error CLAMP NULL-> Y+V impossibile')
|
|
end
|
|
-- [C] richiesto solo carrello V
|
|
elseif ( not dYDeltaF) and dVDeltaF then
|
|
-- [C1] provengo da solo carrello Y
|
|
if dYDeltaI and ( not dVDeltaI) then
|
|
return SpecAdjustCarrC1( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta)
|
|
-- [C2] provengo da carrelli Y e V
|
|
elseif dYDeltaI and dVDeltaI then
|
|
return SpecAdjustCarrC2( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta)
|
|
-- [C3] rimango sul carrello V
|
|
elseif ( not dYDeltaI) and dVDeltaI then
|
|
return SpecAdjustCarrC3( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta, bFixedPos)
|
|
else
|
|
EgtOutLog( ' Error CLAMP NULL-> V impossibile')
|
|
error( 'Error CLAMP NULL-> V impossibile')
|
|
end
|
|
end
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistN, sSide)
|
|
local dRecTotn = min( dDistN, dCorsaYfc+ dCorsaVfc) -- recupero possibile al netto di dCorsa..TrA)
|
|
local dCorsaYd, dCorsaYTd, dCorsaVd, dCorsaVTd
|
|
if sSide == 'Fr' then
|
|
if dRecTotn / 2 <= dCorsaYfc then
|
|
if dRecTotn / 2 <= dCorsaVfc then
|
|
dCorsaYTd = dRecTotn / 2
|
|
dCorsaVd = dRecTotn / 2
|
|
else
|
|
dCorsaVd = dCorsaVfc
|
|
dCorsaYTd = min( dCorsaYfc, dRecTotn - dCorsaVd)
|
|
end
|
|
else
|
|
dCorsaYTd = dCorsaYfc
|
|
dCorsaVd = min( dCorsaVfc, dRecTotn - dCorsaYTd)
|
|
end
|
|
dCorsaY = dCorsaYTd
|
|
dCorsaV = dCorsaVd
|
|
else
|
|
if dRecTotn / 2 <= dCorsaVfc then
|
|
if dRecTotn / 2 <= dCorsaYfc then
|
|
dCorsaVTd = dRecTotn / 2
|
|
dCorsaYd = dRecTotn / 2
|
|
else
|
|
dCorsaYd = dCorsaYfc
|
|
dCorsaVTd = min( dCorsaVfc, dRecTotn - dCorsaYd)
|
|
end
|
|
else
|
|
dCorsaVTd = dCorsaVfc
|
|
dCorsaYd = min( dCorsaYfc, dRecTotn - dCorsaVTd)
|
|
end
|
|
dCorsaY = dCorsaYd
|
|
dCorsaV = dCorsaVTd
|
|
end
|
|
return dCorsaY, dCorsaV
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function PosxExtraYV( dYPosA, dVPosA, dTPosA, dMinY, dMaxV, sYV)
|
|
-- svolge la predisposizione iniziale di trave e carrelli in caso di situazione iniziale con un solo carrello chiuso
|
|
-- con un solo carrello chiuso per l'aggancio del carrello aperto
|
|
local dYPos, dVPos, dTPos
|
|
-- per aggancio del carrello V
|
|
if sYV == 'V' then
|
|
dTPos = min( dTPosA, dVPosA - MinJoin - EgtIf( EMC.HCING_IGNORE, 0, EMC.HCING) - EMC.HOVM)
|
|
dYPos = dYPosA + ( dTPos - dTPosA)
|
|
if dYPos < dMinY then
|
|
dVPos = dVPosA + ( dMinY - dYPos)
|
|
if dVPos > dMaxV + GEO.EPS_SMALL then
|
|
EmitComment( vCmd, ' Error CLAMP V')
|
|
error( 'Error CLAMP V')
|
|
return
|
|
elseif dVPos > dMaxV - GEO.EPS_SMALL then
|
|
dVPos = dMaxV
|
|
end
|
|
dYPos = dMinY
|
|
dTPos = dTPosA + ( dYPos - dYPosA)
|
|
else
|
|
--dYPos = dMinY
|
|
--dYPos = dYPos!
|
|
--dTPos = dTPosA + ( dYPos - dYPosA)
|
|
--dTPos = dTPos!
|
|
dVPos = dVPosA
|
|
end
|
|
end
|
|
-- per aggancio del carrello Y
|
|
if sYV == 'Y' then
|
|
dTPos = max( dTPosA, dYPosA + MinJoin + EMC.TCING - EMC.LB)
|
|
dVPos = dVPosA + ( dTPos - dTPosA)
|
|
if dVPos > dMaxV then
|
|
dYPos = dYPosA - ( dVPos - dMaxV)
|
|
if dYPos < dMinY - GEO.EPS_SMALL then
|
|
EmitComment( vCmd, ' Error CLAMP Y')
|
|
error( 'Error CLAMP Y')
|
|
return
|
|
elseif dYPos < dMinY + GEO.EPS_SMALL then
|
|
dYPos = dMinY
|
|
end
|
|
dVPos = dMaxV
|
|
dTPos = dTPosA + ( dVPos - dVPosA)
|
|
else
|
|
-- ++++++++++++++++++++++++++++++++
|
|
--dVPos = dMaxV
|
|
--dVPos = dVPos!
|
|
--dTPos = dTPosA + ( dVPos - dVPosA)
|
|
--dTpos = dTPos!!
|
|
dYPos = dYPosA
|
|
end
|
|
end
|
|
return dYPos, dVPos, dTPos
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
local function PosXs2Enl (dYa, dVa, dTa, dExtraC, dCorsaTra, dCorsaYd, dCorsaVd, sYV)
|
|
-- svolge la fase di allontanamento dei carrelli per il recupero di 'ExtraV'/'ExtraY'
|
|
-- richiesto alle posizioni [--.xs2] delle funzioni SpecAdjustCarr..
|
|
if sYV == 'V' then -- caso di ExtraV (ed eventuale ulteriore allontanamento di Y)
|
|
local dCorsaYTrA = dCorsaTra
|
|
local dExtraVn = dExtraC - dCorsaYTrA
|
|
-- eseguo allontanamento di Y e (V+T)
|
|
if dExtraVn / 2 <= dCorsaYd then
|
|
if dExtraVn / 2 <= dCorsaVd then
|
|
dYa = dYa + dExtraVn / 2
|
|
dVa = dVa - dExtraVn / 2
|
|
dTa = dTa - dExtraVn / 2
|
|
else
|
|
dVa = dVa - dCorsaVd
|
|
dTa = dTa - dCorsaVd
|
|
dYa = dYa + min( dExtraVn - dCorsaVd, dCorsaYd)
|
|
end
|
|
else
|
|
dYa = dYa + dCorsaYd
|
|
dVa = dVa - min( dExtraVn - dCorsaYd, dCorsaVd)
|
|
dTa = dTa - min( dExtraVn - dCorsaYd, dCorsaVd)
|
|
end
|
|
end
|
|
|
|
if sYV == 'Y' then -- caso di ExtraY (ed eventuale ulteriore allontanamento di V)
|
|
local dCorsaVTrA = dCorsaTra
|
|
local dExtraYn = -dExtraC - dCorsaVTrA
|
|
-- eseguo allontanamento di (Y+T) e V
|
|
if dExtraYn / 2 <= dCorsaVd then
|
|
if dExtraYn / 2 <= dCorsaYd then
|
|
dVa = dVa - dExtraYn / 2
|
|
dYa = dYa + dExtraYn / 2
|
|
dTa = dTa + dExtraYn / 2
|
|
else
|
|
dYa = dYa + dCorsaYd
|
|
dTa = dTa + dCorsaYd
|
|
dVa = dVa - min( dExtraYn - dCorsaYd, dCorsaVd)
|
|
end
|
|
else
|
|
dVa = dVa - dCorsaVd
|
|
dYa = dYa + min( dExtraYn - dCorsaVd, dCorsaYd)
|
|
dTa = dTa + min( dExtraYn - dCorsaVd, dCorsaYd)
|
|
end
|
|
end
|
|
return dYa, dVa, dTa
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
-- *** [A1] da carrello Y a Y : Y -> Y ***
|
|
---------------------------------------------------------------------
|
|
function
|
|
SpecAdjustCarrA1( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF)
|
|
EgtOutLog( ' *[A1] = Y -> Y', 1)
|
|
-- elenco comandi
|
|
local vCmd = {}
|
|
-- Commento
|
|
table.insert( vCmd, { 0, 'Y -> Y'})
|
|
-- se primo scambio
|
|
local MyMinY = EgtIf( EMC.CNT == 1, MinY + AGG_LOAD, MinY)
|
|
-- recupero le posizioni correnti dei carrelli
|
|
local dYPosA = dTPosI + dYDeltaI
|
|
local dVPosA = ParkV
|
|
local dTPosA = dTPosI
|
|
local dYDeltaA = dYDeltaI
|
|
local dVDeltaA = dVPosA - dTPosA
|
|
local dNewYDelta -- = dYDeltaF
|
|
local dCorsaYfc = MaxY - dYPosA
|
|
local dCorsaYd = min( dCorsaYfc, EMC.LB - dYDeltaI - MinJoin)
|
|
|
|
-- tolleranza
|
|
local dYDeltaTol = GetDeltaTol( EMC.LB - dYDeltaF, EMC.TCING, EMC.HCING, EMC.HOVM, 'Y')
|
|
local bYDeltaS = ( dYDeltaF < dYDeltaA - dYDeltaTol or dYDeltaF > dYDeltaA + DELTA_SIC)
|
|
|
|
if bYDeltaS then
|
|
dNewYDelta = dYDeltaF + dYDeltaTol/2
|
|
else
|
|
dNewYDelta = dYDeltaA
|
|
end
|
|
EgtOutLog( ' YDeltaI=' .. EgtNumToString( dYDeltaI) .. ' TPosI=' .. EgtNumToString( dTPosI), 1)
|
|
EgtOutLog( ' YDeltaF=' .. EgtNumToString( dYDeltaF) .. ' NewYDelta=' .. EgtNumToString( dNewYDelta), 1)
|
|
-- flag per risalita resta a Zmax
|
|
local bZmaxOk = false
|
|
|
|
-- **[A1Ys]** |pos. di Y cambia in modo significativo|
|
|
if bYDeltaS then
|
|
-- eventuale risalita testa a Zmax
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
-- definisco 'ExtraY' con (Y+T) e V accentrati q.b. per la presa con V
|
|
local dYPos, dVPos, dTPos = PosxExtraYV( dYPosA, dVPosA, dTPosA, MyMinY, MaxV, 'V')
|
|
local dNewY = dTPos + dNewYDelta
|
|
local dExtraY = dNewY - MyMinY
|
|
-- effettuo spostamenti di (Y+T) e V per predisporre all'aggancio di T con V
|
|
dYPosA = dYPos
|
|
dVPosA = dVPos
|
|
dTPosA = dTPos
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
dVDeltaA = dVPosA - dTPosA
|
|
|
|
-- **[A1Ys-x]** posizione di |Y non raggiungibile| (oltre MyMinY)
|
|
if dExtraY < 0 then
|
|
-- **[A1Ys-xs]** posizione di Y non raggiungibile, |con ExtraY 'significativo'|
|
|
if -dExtraY > dYDeltaTol /2 then
|
|
--EmitComment( vCmd, '[A1Ys-xs]')
|
|
-- calcolo le **corse disponibili dei carrelli a partire da Y e V c.s.** per allontanare q.p. (Y+T) e V
|
|
local dCorsaYfc = MaxY - dYPos
|
|
local dCorsaVfc = dVPos - MinV
|
|
local dDistFrN = (dVPos-dTPos) - MinJoin - EMC.HCING - EMC.HOVM -- DistFront 'netta'
|
|
local dCorsaYTd, dCorsaVd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistFrN, 'Fr')
|
|
-- valuto **le corse di 'recupero'** dai due carrelli possibili riaccentrando Y e (V+T)
|
|
local dCorsaVTr = dCorsaVd + (MaxV - dVPos)
|
|
local dCorsaYr = dCorsaYTd
|
|
-- **[A1Ys-xsw]** posizione finale dNewV non raggiungibile, con |dEXtraY > CorsaVr + CorsaYr|
|
|
if -dExtraY > ( dCorsaVTr + dCorsaYr) and bYDeltaS then
|
|
EmitComment( vCmd, '[A1Ys-xsw]' .. 'CASO NON GESTITO')
|
|
return
|
|
end
|
|
|
|
local dCorsaVTrA = MaxV - dVPos -- !! att.ne: non dVPosA !!
|
|
-- **[A1Ys-xs1]** se posso recuperare ExtraY semplicem' accentrando Y e (V+T)
|
|
-- (dalle posizione impostate sopra per l'aggancio di V)
|
|
if dCorsaVTrA >= -dExtraY then
|
|
EmitComment( vCmd, '[A1Ys-xs1]')
|
|
-- chiudo V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **1: accentro** Y e (V+T)
|
|
dYPosA = MyMinY -- (pos. finale)
|
|
dVPosA = dVPosA + (-dExtraY)
|
|
dTPosA = dTPosA + (-dExtraY)
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
dYDeltaA = dYPosA - dTPosA
|
|
|
|
else -- **[A1Ys-xs2]**
|
|
-- ci sarebbe un doppio movimento di Y ? => caso impossibile ?
|
|
EmitComment( vCmd, '[A1Ys-xs2]')
|
|
-- **1:** posiziono (ulteriormente!) (Y+T) e V
|
|
dYPosA, dVPosA, dTPosA = PosXs2Enl( dYPosA, dVPosA, dTPosA, dExtraY, dCorsaVTrA, dCorsaYTd, dCorsaVd, 'Y')
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA })
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- chiudo V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **2: accentro** Y e (V+T)
|
|
dYPosA = MyMinY -- (pos. finale)
|
|
dVPosA = MaxV
|
|
dTPosA = dVPosA - dVDeltaA
|
|
--dVDeltaA = dVPosA - dTPosA
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
end -- [A1Ys-xs1/-xs2]
|
|
|
|
-- **[A1Ys-xn]** |dExtraY ancora < 0 ma non 'significativo')|
|
|
else --if -dExtraY < DeltaToll/2
|
|
EmitComment( vCmd, '[A1Ys-xn]')
|
|
-- chiudo V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **1:** accentro Y
|
|
dExtraY = 0
|
|
dYPosA = MyMinY -- (pos. finale)
|
|
table.insert( vCmd, { 1, 'Y', dYPosA})
|
|
-- dYDeltaA = dYPosA - dTPosA
|
|
end --[A1Ys-xs/xn]
|
|
|
|
-- **[A1Ys-r]** |posizione di Y raggiungibile| (ExtraY >=0)
|
|
-- (si esclude la possibilità di extra corsa oltre maxY)
|
|
else
|
|
EmitComment( vCmd, '[A1Ys-r]')
|
|
-- chiudo V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **1:** posiziono Y alla posizione richiesta
|
|
dYPosA = dNewY
|
|
table.insert( vCmd, { 1, 'Y', dYPosA})
|
|
end -- [A1Ys]
|
|
|
|
-- calcolo il nuovo parametro di aggancio
|
|
dYDeltaA = dYPosA - dTPosA
|
|
-- chiudo Y e apro V
|
|
table.insert( vCmd, { 11, CalcCharStatus( 'Y', dYDeltaA)})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- sposto il carrello V in parcheggio
|
|
table.insert( vCmd, { 1, 'V', ParkV})
|
|
-- imposto il nuovo parametro di aggancio
|
|
table.insert( vCmd, { 21, dYDeltaA, 0})
|
|
|
|
-- reset contatore
|
|
EMC.CNT = nil
|
|
|
|
else -- **[A1Yns]** |spostamento| finale richiesto (ev' residuo) di |Y non 'significativo'|
|
|
EmitComment( vCmd, '[A1Yns]')
|
|
end --[A1Ys/ns]
|
|
|
|
EgtOutLog(' YDeltaA =' .. EgtNumToString( dYDeltaA), 1)
|
|
|
|
SpecOutputCNT()
|
|
return vCmd
|
|
end --SpecAdjustCarrA1
|
|
|
|
---------------------------------------------------------------------
|
|
-- *** [A2] da entrambi a carrello Y : Y+V -> Y ***
|
|
---------------------------------------------------------------------
|
|
function SpecAdjustCarrA2( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF)
|
|
EgtOutLog( ' *[A2] = Y+V -> Y ', 1)
|
|
-- elenco comandi
|
|
local vCmd = {}
|
|
-- Commento
|
|
table.insert( vCmd, { 0, 'Y+V -> Y'})
|
|
-- se primo scambio
|
|
local MyMinY = EgtIf( EMC.CNT == 1, MinY + AGG_LOAD, MinY)
|
|
local dCorsaY = MaxY - MyMinY
|
|
local dCorsaV = MaxV - MinV
|
|
-- recupero le posizioni correnti dei carrelli
|
|
local dYPosA = dTPosI + dYDeltaI
|
|
local dVPosA = dTPosI + dVDeltaI
|
|
local dTPosA = dTPosI
|
|
local dYDeltaA = dYDeltaI
|
|
local dVDeltaA = dVDeltaI
|
|
local dNewYDelta -- = dYDeltaF
|
|
-- tolleranze
|
|
local dYDeltaTol = GetDeltaTol( EMC.LB - dYDeltaF, EMC.TCING, EMC.HCING, EMC.HOVM, 'Y')
|
|
local bYDeltaS = ( dYDeltaF < dYDeltaA - dYDeltaTol or dYDeltaF > dYDeltaA + DELTA_SIC)
|
|
|
|
if bYDeltaS then
|
|
dNewYDelta = dYDeltaF + dYDeltaTol/2
|
|
else
|
|
dNewYDelta = dYDeltaA
|
|
end
|
|
-- definisco 'ExtraY'
|
|
local dNewY = dTPosA + dNewYDelta
|
|
local dExtraY = dNewY - MyMinY -- < 0 = nuova pos. di Y 'non raggiungibile' (= oltre MyMinY)
|
|
-- definisco 'ExtraY significativo'
|
|
local bYxs = -dExtraY > dYDeltaTol/2
|
|
|
|
EgtOutLog( ' YDeltaI=' .. EgtNumToString( dYDeltaI) .. ' TPosI=' .. EgtNumToString( dTPosI) ..
|
|
' VDeltaI=' .. EgtNumToString( dVDeltaI), 1)
|
|
EgtOutLog( ' YDeltaF=' .. EgtNumToString( dYDeltaF) .. ' NewYDelta=' .. EgtNumToString( dNewYDelta), 1)
|
|
-- flag per risalita resta a Zmax
|
|
local bZmaxOk = false
|
|
-- risalita testa a Zmax (da effettuare comunque, per il parcheggio di V)
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
|
|
-- calcoli preliminari in caso di spostamento richiesto per Y significativo
|
|
local dCorsaYfc, dCorsaVfc, dDistFrN, dCorsaYTd, dCorsaVd, dCorsaVTr, dCorsaYr, bXsw
|
|
if bYDeltaS then
|
|
if bYxs then
|
|
-- calcolo le **corse disponibili** dei carrelli **a partire dalle posizioni attuali** (=iniziali)
|
|
-- per allontanare q.p. (Y+T) e V
|
|
dCorsaYfc = MaxY - dYPosA
|
|
dCorsaVfc = dVPosA - MinV
|
|
dDistFrN = dVDeltaA - MinJoin - EMC.HCING - EMC.HOVM -- DistFront 'netta'
|
|
dCorsaYTd, dCorsaVd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistFrN, 'Fr')
|
|
-- valuto **le corse di 'recupero'** dai due carrelli possibili riaccentrando Y e (V+T)
|
|
dCorsaVTr = dCorsaVd + (MaxV - dVPosA)
|
|
dCorsaYr = dCorsaYTd
|
|
bXsw = bYDeltaS and bYxs and -dExtraY > dCorsaYr + dCorsaVTr
|
|
end
|
|
end
|
|
|
|
-- **[A2Ys-xsw]** posizione NewY non raggiungibile (oltre MyMinY), con ||dEXtraY | > CorsaYr + CorsaVTr|
|
|
while bXsw do
|
|
EmitComment( vCmd, '[A2Ys-xsw]')
|
|
-- risalita testa a Zmax
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
-- chiudo eventualmente il carrello Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **1:** allontano q.p. (Y+T) e V
|
|
dYPosA = dYPosA + dCorsaYTd
|
|
dTPosA = dTPosA + dCorsaYTd
|
|
dVPosA = dVPosA - dCorsaVd
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
-- chiudo il carrello V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **2:** accentro Y e (V+T)
|
|
dYPosA = MyMinY
|
|
dVPosA = dVPosA + dCorsaVTr
|
|
dTPosA = dTPosA + dCorsaVTr
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
-- valuto Delta attuali
|
|
dYDeltaA = dYPosA - dTPosA
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- aggiorno la verifica di spostamento significativo
|
|
bYDeltaS = ( dYDeltaF < dYDeltaA - dYDeltaTol or dYDeltaF > dYDeltaA + DELTA_SIC)
|
|
-- aggiorno l'extra corsa residua per Y
|
|
dExtraY = dExtraY + dCorsaYr + dCorsaVTr
|
|
bYxs = -dExtraY > dYDeltaTol/2
|
|
-- **aggiorno la valutazione delle corse disponibili**
|
|
-- a partire da Y e V c.s. per allontanare q.p. (Y+T) e V
|
|
dCorsaYfc = MaxY - dYPosA
|
|
dCorsaVfc = dVPosA - MinV
|
|
dDistFrN = dVDeltaA - MinJoin - EMC.HCING - EMC.HOVM -- DistFront 'netta'
|
|
dCorsaYTd, dCorsaVd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistFrN, 'Fr')
|
|
-- valuto le corse di 'recupero' dai due carrelli possibili riaccentrando Y e (V+T)
|
|
dCorsaVTr = dCorsaVd
|
|
dCorsaYr = dCorsaYTd
|
|
-- aggiorno verifica per ripetizione del ciclo
|
|
bXsw = bYDeltaS and bYxs and -dExtraY > dCorsaVTr + dCorsaYr
|
|
end -- [A2Ys-xw]
|
|
|
|
-- **[A2Ys]** se lo |spostamento| richiesto (ev' residuo) per |Y| è |'significativo'|
|
|
if bYDeltaS then
|
|
-- **[A2Ys-x]** se la |posizione per Y non è 'raggiungibile'|
|
|
if dExtraY < 0 then
|
|
-- **[A2Ys-xs]** se |ExtraY 'significativo'|
|
|
if bYxs then
|
|
local dCorsaVTrA = MaxV - dVPosA
|
|
-- **[A2Ys-xs1]** se posso recuperare ExtraY solo accentrando (V+T) e Y
|
|
if dCorsaVTrA >= -dExtraY then
|
|
EmitComment( vCmd, '[A2Ys-xs1]')
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **1:** accentro (V+T) q.b. e Y
|
|
dVPosA = dVPosA + (-dExtraY)
|
|
dTPosA = dTPosA + (-dExtraY)
|
|
dYPosA = MyMinY -- (pos. finale)
|
|
--dYDeltaA = dYPosA - dTPosA
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
else -- **[A2Ys-xs2]**
|
|
EmitComment( vCmd, '[A2Ys-xs2]')
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **1:** allontano (Y+T) e V
|
|
dYPosA, dVPosA, dTPosA = PosXs2Enl( dYPosA, dVPosA, dTPosA, dExtraY, dCorsaVTrA, dCorsaYTd, dCorsaVd, 'Y')
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA })
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- chiudo V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **2:** accentro Y e (V+T) q.b. per recupero di dExtraY
|
|
dYPosA = MyMinY
|
|
dVPosA = MaxV
|
|
dTPosA = dVPosA - dVDeltaA
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
end --[A2Ys-xs1/-xs2]
|
|
|
|
-- [A2Ys-xn] |ExtraY ancora <= 0, ma 'non significativo'|
|
|
else --if dExtraY <= 0 then
|
|
EmitComment( vCmd, '[A2Ys-xn]')
|
|
dExtraY = 0
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- 1: accentro Y
|
|
dYPosA = MyMinY -- (pos. finale)
|
|
table.insert( vCmd, { 1, 'Y', dYPosA})
|
|
end -- [A2Ys-xs/xn]
|
|
|
|
--[A2Ys-r] dExtrav > 0 ( |pos. Y'raggiungibile|)
|
|
else
|
|
EmitComment( vCmd, '[A2Ys-r]')
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- posizione Y
|
|
dYPosA = dTPosA + dNewYDelta -- (pos. finale)
|
|
table.insert( vCmd, { 1, 'Y', dYPosA})
|
|
end -- [A2Ys-x/r]
|
|
|
|
else -- [A2Yns] |spostamento Y| (ev' residuo) |non significativo|
|
|
EmitComment( vCmd, '[A2Yns]')
|
|
end -- [A2Ys/ns]
|
|
|
|
-- calcolo i nuovi parametri di aggancio
|
|
dYDeltaA = dYPosA - dTPosA
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, CalcCharStatus( 'Y', dYDeltaA)})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- sposto il carrello V in parcheggio
|
|
table.insert( vCmd, { 1, 'V', ParkV})
|
|
-- imposto i nuovi parametri di aggancio
|
|
table.insert( vCmd, { 21, dYDeltaA, 0})
|
|
|
|
EgtOutLog( ' YDeltaA =' .. EgtNumToString( dYDeltaA), 1)
|
|
-- reset contatore
|
|
EMC.CNT = nil
|
|
|
|
SpecOutputCNT()
|
|
return vCmd
|
|
end --SpecAdjustCarrA2
|
|
|
|
---------------------------------------------------------------------
|
|
-- *** [A3] da carrello V a Y : V -> Y ***
|
|
---------------------------------------------------------------------
|
|
function SpecAdjustCarrA3( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF)
|
|
EgtOutLog( ' *[A3] = V -> Y', 1)
|
|
-- elenco comandi
|
|
local vCmd = {}
|
|
-- Commento
|
|
table.insert( vCmd, { 0, 'V -> Y'})
|
|
-- se primo scambio
|
|
local MyMinY = EgtIf( EMC.CNT == 1, MinY + AGG_LOAD, MinY)
|
|
-- recupero le posizioni correnti
|
|
local dYPosA = ParkY
|
|
local dTPosA = dTPosI
|
|
local dVPosA = dTPosI + dVDeltaI
|
|
local dYDeltaA = dYPosA - dTPosA
|
|
local dVDeltaA = dVDeltaI
|
|
local dNewYDelta -- = dVDeltaF
|
|
-- tolleranza
|
|
local dYDeltaTol = GetDeltaTol( EMC.LB - dYDeltaF, EMC.TCING, EMC.HCING, EMC.HOVM, 'Y')
|
|
local bYDeltaS = ( dYDeltaF < dYDeltaA - dYDeltaTol or dYDeltaF > dYDeltaA + DELTA_SIC)
|
|
-- reimposto i delta finali in caso di spostamenti richiesti significativi, tenuto conto delle tolleranze
|
|
if bYDeltaS then
|
|
dNewYDelta = dYDeltaF + dYDeltaTol/2
|
|
else
|
|
dNewYDelta = dYDeltaA
|
|
end
|
|
--
|
|
EgtOutLog(' VDeltaI=' .. EgtNumToString( dVDeltaA) .. ' YDeltaI(Park)='.. EgtNumToString( dYDeltaA)..
|
|
' TPosI=' .. EgtNumToString( dTPosI) , 1)
|
|
EgtOutLog(' YDeltaF=' .. EgtNumToString( dYDeltaF) .. ' NewYDelta=' .. EgtNumToString( dNewYDelta) , 1)
|
|
local bZmaxOk = false
|
|
-- risalita testa a Zmax (da effettuare comunque, dato lo scambio di carrelli)
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
|
|
-- calcoli preliminari in caso di spostamento richiesto per Y significativo
|
|
local dYPos, dVPos, dTPos
|
|
local dNewY, dExtraY, bYxs
|
|
local dCorsaYfc, dCorsaVfc, dDistFrN, dCorsaYTd, dCorsaVd, dCorsaVTr, dCorsaYr, bXsw
|
|
if bYDeltaS then
|
|
-- definisco 'ExtraY' con Y e (V+T) accentrati q.b. per la presa con Y
|
|
dYPos, dVPos, dTPos = PosxExtraYV( dYPosA, dVPosA, dTPosA, MyMinY, MaxV, 'Y')
|
|
dNewY = dTPos + dNewYDelta
|
|
dExtraY = dNewY - MyMinY --( <0 <=> pos. 'non raggiungibile')
|
|
bYxs = -dExtraY > dYDeltaTol/2
|
|
if bYxs then
|
|
-- calcolo le **corse disponibili dei carrelli a partire da Y e V c.s.** per allontanare q.p. (Y+T) e V
|
|
dCorsaYfc = MaxY - dYPos
|
|
dCorsaVfc = dVPos - MinV
|
|
--local dDistFrN = dVDeltaA - MinJoin - EMC.HCING - EMC.HOVM -- DistFront 'netta'
|
|
dDistFrN = (dVPos-dTPos) - MinJoin - EMC.HCING - EMC.HOVM -- DistFront 'netta'
|
|
dCorsaYTd, dCorsaVd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistFrN, 'Fr')
|
|
-- valuto **le corse di 'recupero'** dai due carrelli possibili riaccentrando Y e (V+T)
|
|
dCorsaVTr = dCorsaVd + (MaxV - dVPos)
|
|
dCorsaYr = dCorsaYTd
|
|
bXsw = bYDeltaS and bYxs and -dExtraY > dCorsaVTr + dCorsaYr
|
|
end
|
|
-- inizializzo
|
|
dYPosA = dYPos
|
|
dVPosA = dVPos
|
|
dTPosA = dTPos
|
|
end
|
|
|
|
-- **[A3Ys-xsw]** posizione finale dNewY non raggiungibile, con |dEXtraY > CorsaVr + CorsaYr|
|
|
while bXsw do
|
|
EmitComment( vCmd, '[A3Ys-xsw]')
|
|
-- |1:| posiziono (V+T) e Y come calcolato sopra
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- chiudo ev' il carrello Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- |2:| allontano (Y+T) e V quanto possibile
|
|
dYPosA = dYPosA + dCorsaYTd
|
|
dTPosA = dTPosA + dCorsaYTd
|
|
dVPosA = dVPosA - dCorsaVd
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- chiudo il carrello V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- |3:| accentro (V+T) e Y
|
|
dVPosA = dVPosA + dCorsaVTr
|
|
dTPosA = dTPosA + dCorsaVTr
|
|
dYPosA = MyMinY
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- valuto i Delta ottenuti
|
|
dYDeltaA = dYPosA - dTPosA
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- aggiorno la verifica di spostamento significativo
|
|
bYDeltaS = ( dYDeltaF < dYDeltaA - dYDeltaTol or dYDeltaF > dYDeltaA + DELTA_SIC)
|
|
-- aggiorno ExtraY
|
|
dExtraY = dExtraY + dCorsaYr + dCorsaVTr
|
|
-- **aggiorno la valutazione delle corse disponibili**
|
|
-- a partire da Y e V c.s. per allontanare q.p. (Y+T) e V
|
|
dCorsaYfc = MaxY - dYPosA
|
|
dCorsaVfc = dVPosA - MinV
|
|
dDistFrN = dVDeltaA - MinJoin - EMC.HCING - EMC.HOVM -- DistFront 'netta'
|
|
dCorsaYTd, dCorsaVd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistFrN, 'Fr')
|
|
-- valuto le corse di 'recupero' dai due carrelli possibili riaccentrando Y e (V+T)
|
|
dCorsaVTr = dCorsaVd
|
|
dCorsaYr = dCorsaYTd
|
|
-- aggiorno verifica per ripetizione del ciclo
|
|
bXsw = bYDeltaS and bYxs and -dExtraY > dCorsaVTr + dCorsaYr
|
|
end --[A3Ys-xw]
|
|
|
|
-- **[A3Ys]** |spostamento| richiesto (ev' residuo) |di Y| |'significativo'|
|
|
if bYDeltaS then
|
|
EmitComment( vCmd, '[A3Ys]')
|
|
-- **[A3Ys-x]** posizione di |Y non raggiungibile|
|
|
if dExtraY < 0 then
|
|
|
|
-- **[A3Ys-xs]** |pos. NewY non raggiungibile, con ExtraY 'significativo'|
|
|
-- ( -dExtraY <= (CorsaYr+CorsaVr) da ciclo precedente )
|
|
if bYxs then
|
|
local dCorsaVTrA = MaxV - dVPosA
|
|
-- **[A3Ys-xs1]** se posso recuperare ExtraY semplicem' accentrando Y e (V+T)
|
|
-- ulteriormente rispetto a YPos e Vpos definiti c.s.
|
|
if dCorsaVTrA >= -dExtraY then
|
|
EmitComment( vCmd, '[A3Ys-xs1]')
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **1: accentro** Y e (V+T)
|
|
dYPosA = MyMinY -- (pos. finale)
|
|
dVPosA = dVPosA - dExtraY
|
|
dTPosA = dTPosA - dExtraY
|
|
dYDeltaA = dYPosA - dTPosA
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
|
|
else -- **[A3Ys-xs2]**
|
|
EmitComment( vCmd, '[A3Ys-xs2]')
|
|
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **1: accentro** ev' Y e (V+T) alle posizioni impostate
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
dYDeltaA = dYPosA - dTPosA
|
|
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **2: allontano** (Y+T) e V
|
|
dYPosA, dVPosA, dTPosA = PosXs2Enl( dYPosA, dVPosA, dTPosA, dExtraY, dCorsaVTrA, dCorsaYTd, dCorsaVd, 'Y')
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA })
|
|
dVDeltaA = dVPosA - dTPosA
|
|
|
|
-- chiudo il carrello V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **3: accentro** Y e (V+T)
|
|
dYPosA = MyMinY -- (pos. finale)
|
|
dVPosA = MaxV
|
|
dTPosA = dVPosA - dVDeltaA
|
|
--dYDeltaA = dYPosA - dTPosA
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
end -- [A3Ys-xs1/-xs2]
|
|
|
|
-- **[A3Ys-xn]** |dExtraY ancora < 0 ma non 'significativo')|
|
|
else --if -dExtraY < DeltaToll/2
|
|
EmitComment( vCmd, '[A3Ys-xn]')
|
|
-- ev' chiudo V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **1:** accentro Y e porto (V+T) alla posizione impostata
|
|
dExtraY = 0
|
|
dYPosA = MyMinY -- (pos. finale)
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
end --[A3Ys-xs/xn]
|
|
|
|
-- **[A3Ys-r]** |posizione di Y raggiungibile (ExtraY >=0)|
|
|
else
|
|
EmitComment( vCmd, '[A3Ys-r]')
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- posizione Y in posizione finale ed ev' (V+T) come impostato sopra
|
|
dYPosA = dTPosA + dNewYDelta
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
end -- [A3Ys]
|
|
|
|
else -- **[A3Yns]** |spostamento| finale richiesto (ev' residuo) di |Y non 'significativo'|
|
|
EmitComment( vCmd, '[A3Yns]')
|
|
end --[A3Ys/ns]
|
|
|
|
-- calcolo il nuovo parametro di aggancio
|
|
dYDeltaA = dYPosA - dTPosA
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, CalcCharStatus( 'Y', dYDeltaA)})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- sposto il carrello V in parcheggio
|
|
table.insert( vCmd, { 1, 'V', ParkV})
|
|
-- imposto il nuovo parametro di aggancio
|
|
table.insert( vCmd, { 21, dYDeltaA, 0})
|
|
|
|
EgtOutLog( ' YDeltaA =' .. EgtNumToString( dYDeltaA), 1)
|
|
-- reset contatore
|
|
EMC.CNT = nil
|
|
|
|
SpecOutputCNT()
|
|
return vCmd
|
|
end --SpecAdjustCarrA3
|
|
|
|
---------------------------------------------------------------------
|
|
local function AdjustPositionsForB( dNewYDelta, dNewVDelta, TCING, HCING, HOVM, bFixedDelta)
|
|
local REF_DIST = 1400
|
|
local dYDelta = 0
|
|
local dVDelta = 0
|
|
-- incremento se possibile la distanza tra le due posizioni
|
|
if not bFixedDelta and ( dNewYDelta - dNewVDelta) < REF_DIST then
|
|
local dEffAddDist = ( REF_DIST - ( dNewYDelta - dNewVDelta)) / 2
|
|
dYDelta = max( min( dEffAddDist, EMC.LB - dNewYDelta - MinJoin - TCING), 0)
|
|
dVDelta = max( min( dEffAddDist, dNewVDelta - MinJoin - HOVM - HCING), 0)
|
|
end
|
|
return dYDelta, dVDelta
|
|
end
|
|
|
|
---------------------------------------------------------------------
|
|
-- *** [B1] da carrello Y a entrambi : Y -> Y+V ***
|
|
---------------------------------------------------------------------
|
|
function SpecAdjustCarrB1( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta)
|
|
EgtOutLog( ' *[B1] = Y -> Y+V', 1)
|
|
-- elenco comandi
|
|
local vCmd = {}
|
|
-- Commento
|
|
table.insert( vCmd, { 0, 'Y -> Y+V'})
|
|
-- se primo scambio
|
|
local MyMinY = EgtIf( EMC.CNT == 1, MinY + AGG_LOAD, MinY)
|
|
local dCorsaY = MaxY - MyMinY
|
|
local dCorsaV = MaxV - MinV
|
|
-- recupero le posizioni correnti
|
|
local dYPosA = dTPosI + dYDeltaI
|
|
local dVPosA = ParkV
|
|
local dTPosA = dTPosI
|
|
local dYDeltaA = dYDeltaI
|
|
local dVDeltaA = dVPosA - dTPosA
|
|
local dNewYDelta
|
|
local dNewVDelta
|
|
-- incremento la distanza tra le due posizioni ( se abilitato e possibile)
|
|
local dYDeltaAgg, dVDeltaAgg = AdjustPositionsForB( dYDeltaF, dVDeltaF, EMC.TCING, EMC.HCING, EMC.HOVM, bFixedDelta)
|
|
-- tolleranze
|
|
local dYDeltaTol = dYDeltaAgg + GetDeltaTol( EMC.LB - dYDeltaF - dYDeltaAgg, EMC.TCING, EMC.HCING, EMC.HOVM, 'Y', bFixedDelta)
|
|
local dVDeltaTol = dVDeltaAgg + GetDeltaTol( dVDeltaF - dVDeltaAgg, EMC.TCING, EMC.HCING, EMC.HOVM, 'V', bFixedDelta)
|
|
-- definisco criteri per movimenti 'significativi' in base alle 'nuove' tolleranze
|
|
local bYDeltaS = ( dYDeltaF < dYDeltaA - dYDeltaTol or dYDeltaF > dYDeltaA + DELTA_SIC)
|
|
local bVDeltaS = ( dVDeltaF > dVDeltaA + dVDeltaTol or dVDeltaF < dVDeltaA - DELTA_SIC)
|
|
-- reimposto i delta finali in caso di spostamenti richiesti significativi, tenuto conto delle tolleranze
|
|
if bYDeltaS then
|
|
dNewYDelta = dYDeltaF + dYDeltaTol / 2
|
|
else
|
|
dNewYDelta = dYDeltaA
|
|
end
|
|
if bVDeltaS then
|
|
dNewVDelta = dVDeltaF - dVDeltaTol / 2
|
|
else
|
|
dNewVDelta = dVDeltaA
|
|
end
|
|
|
|
EgtOutLog(' YDeltaI=' .. EgtNumToString( dYDeltaA) .. ' VDeltaI(Park)='.. EgtNumToString( dVDeltaA) ..
|
|
' TPosI='.. EgtNumToString( dTPosI), 1)
|
|
EgtOutLog(' YDeltaF=' .. EgtNumToString( dYDeltaF) .. ' VDeltaF=' .. EgtNumToString( dVDeltaF), 1)
|
|
EgtOutLog(' NewYDelta=' .. EgtNumToString( dNewYDelta) .. ' NewVDelta=' .. EgtNumToString( dNewVDelta), 1)
|
|
|
|
-- risalita testa a Zmax
|
|
local bZmaxOk = false
|
|
|
|
-- calcoli preliminari in caso di spostamento richiesto per V significativo
|
|
local dYPos, dVpos, dTPos
|
|
local dNewV, dExtraV, bVxs
|
|
local dCorsaYfc, dCorsaVfc, dDistBkN, dCorsaYd, dCorsaVTd, dCorsaYTr, dCorsaVr, bXsw
|
|
if bVDeltaS then
|
|
-- definisco 'ExtraV' con (Y+T) e V accentrati q.b. per la presa di V
|
|
dYPos, dVPos, dTPos = PosxExtraYV( dYPosA, dVPosA, dTPosA, MyMinY, MaxV, 'V')
|
|
dNewV = dTPos + dNewVDelta
|
|
dExtraV = dNewV - MaxV -- se > 0 pos. V non direttamente 'raggiungibile'
|
|
bVxs = dExtraV > dVDeltaTol/2 -- ExtraV 'significativo'
|
|
if bVxs then
|
|
-- calcolo le **corse disponibili** dei carrelli a partire da Y e V c.s.|
|
|
-- per allontanare q.p. Y e (V+T)
|
|
dCorsaYfc = MaxY - dYPos
|
|
dCorsaVfc = dVPos - MinV
|
|
dDistBkN = EMC.LB - dYDeltaA - MinJoin - EMC.TCING -- DistBack 'netta'
|
|
dCorsaYd, dCorsaVTd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistBkN, 'Bk')
|
|
-- valuto le **corse di 'recupero'** dai due carrelli possibili riaccentrando (Y+T) e V
|
|
dCorsaYTr = dCorsaYd + (dYPos - MyMinY)
|
|
dCorsaVr = dCorsaVTd
|
|
bXsw = bVDeltaS and bVxs and dExtraV > dCorsaVr + dCorsaYTr
|
|
end
|
|
-- inizializzo
|
|
dYPosA = dYPos
|
|
dVPosA = dVPos
|
|
dTPosA = dTPos
|
|
end
|
|
|
|
-- **[B1Vs-xsw]** posizione finale dNewV non raggiungibile, con |dEXtraV > CorsaVr + CorsaYr|
|
|
while bXsw do
|
|
EmitComment( vCmd, '[B1Vs-xsw]')
|
|
-- risalita testa a Zmax
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
|
|
-- |1:| imposto (Y+T) e V come calcolato sopra
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- |2:| allontano Y e (V+T) quanto possibile
|
|
dYPosA = dYPosA + dCorsaYd
|
|
dVPosA = dVPosA - dCorsaVTd
|
|
dTPosA = dTPosA - dCorsaVTd
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- chiudo il carrello Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- |3:| accentro (Y+T) e V
|
|
dYPosA = dYPosA - dCorsaYTr
|
|
dTPosA = dTPosA - dCorsaYTr
|
|
dVPosA = MaxV
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- valuto i Delta ottenuti
|
|
dYDeltaA = dYPosA - dTPosA
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- aggiorno la verifica di spostamento significativo
|
|
bYDeltaS = ( dYDeltaF < dYDeltaA - dYDeltaTol or dYDeltaF > dYDeltaA + DELTA_SIC)
|
|
bVDeltaS = ( dVDeltaF > dVDeltaA + dVDeltaTol or dVDeltaF < dVDeltaA - DELTA_SIC)
|
|
-- aggiorno l'extra corsa residuo per V
|
|
dExtraV = dExtraV - dCorsaYTr - dCorsaVr
|
|
|
|
-- **aggiorno la valutazione delle corse disponibili**
|
|
-- a partire da Y e V c.s. per allontanare q.p. Y e (V+T)
|
|
dCorsaYfc = MaxY - dYPosA
|
|
dCorsaVfc = dVPosA - MinV
|
|
dDistBkN = EMC.LB - dYDeltaA - MinJoin - EMC.TCING -- DistBack 'netta'
|
|
dCorsaYd, dCorsaVTd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistBkN, 'Bk')
|
|
-- aggiorno i prossimi recuperi disponibili
|
|
dCorsaYTr = dCorsaYd
|
|
dCorsaVr = dCorsaVTd
|
|
-- aggiorno verifica per ripetizione del ciclo
|
|
bXsw = bVDeltaS and bVxs and dExtraV > dCorsaVr + dCorsaYTr
|
|
end --[B1Vs-xw]
|
|
|
|
-- **[B1Vs]** se lo |spostamento| finale richiesto (ev' residuo) di |V| è |'significativo'|
|
|
if bVDeltaS then
|
|
-- risalita testa a Zmax
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
-- **[B1Vs-x]** posizione di |V non raggiungibile|
|
|
if dExtraV > 0 then
|
|
-- **[B1Vs-xs]** |ExtraV 'signfificativo'|
|
|
-- (ExtraV <= (CorsaYr+CorsaVr) da ciclo precedente )
|
|
if bVxs then
|
|
local dCorsaYTrA = dYPosA - MyMinY
|
|
local dCorsaVra = MaxV - dVPosA
|
|
-- **[B1Vs-xs1]** se posso recuperare ExtraV semplicem' accentrando V e (Y+T)
|
|
-- ulteriormente rispetto a YPos e Vpos definiti c.s.
|
|
if dCorsaYTrA >= dExtraV then
|
|
EmitComment( vCmd, '[B1Vs-xs1]')
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **1: accentro** (Y+T) e V
|
|
dYPosA = dYPosA - dExtraV
|
|
dTPosA = dTPosA - dExtraV
|
|
dVPosA = MaxV -- (pos. finale)
|
|
dVDeltaA = dVPosA - dTPosA
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
|
|
else -- **[B1Vs-xs2]**
|
|
EmitComment( vCmd, '[B1Vs-xs2]')
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **1: accentro** ev' (Y+T) e V alle posizioni impostate
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
dVDeltaA = dVPosA - dTPosA
|
|
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **2: allontano** (V+T) e Y
|
|
-- tenendo conto di dover 'recuperare' ExtraV....
|
|
local dYPos1, dVPos1, dTPos1 = PosXs2Enl( dYPosA, dVPosA, dTPosA, dExtraV, dCorsaYTrA, dCorsaYd, dCorsaVTd, 'V')
|
|
local dYDispl1 = dYPos1 - dYPosA
|
|
local dVDispl1 = dVPos1 - dVPosA
|
|
--local dTDispl1 = dTPos1 - dTPosA
|
|
local dYDeltaDiff = dNewYDelta - (dYPos1 - dTPos1)
|
|
-- ...e anche di posizionare possibilmente Y alla posizione finale
|
|
if dYDeltaDiff > 0 then
|
|
dCorsaYfc = MaxY - dYPos1
|
|
dCorsaVfc = dVPos1 - MinV
|
|
dDistBkN = EMC.LB - (dYPos1-dTPos1) - MinJoin - EMC.TCING -- DistBack1 'netta'
|
|
dCorsaYd, dCorsaVTd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistBkN, 'Bk')
|
|
dYPosA, dVPosA, dTPosA = PosXs2Enl( dYPos1, dVPos1, dTPos1, dYDeltaDiff, 0, dCorsaYd, dCorsaVTd, 'V')
|
|
else
|
|
dYPosA = dYPos1; dVPosA = dVPos1; dTPosA = dTPos1
|
|
end
|
|
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA })
|
|
|
|
-- chiudo il carrello Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **3: accentro** (Y+T) e V
|
|
dYPosA = dYPosA - dYDispl1 - dCorsaYTrA -- POS. FINALE
|
|
dTPosA = dTPosA - dYDispl1 - dCorsaYTrA
|
|
dVPosA = dVPosA - dVDispl1 + dCorsaVra -- POS. FINALE
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
end -- [B1Vs-xs1/-xs2]
|
|
|
|
-- **[B1Vs-xn]** |dExtraV ancora > 0 ma non 'significativo')|
|
|
else --if dExtraV < DeltaToll/2
|
|
EmitComment( vCmd, '[B1Vs-xn]')
|
|
-- ev' chiudo Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- 1: accentro (Y+T) alla posizione impostate e V al max
|
|
dExtraV = 0
|
|
dVPosA = MaxV -- (pos. finale)
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
end -- [B1s-xs/xn]
|
|
|
|
-- **[B1Vs-r]** |dExtraV < 0 (pos. V raggiungibile)|
|
|
else
|
|
EmitComment( vCmd, '[B1Vs-r]')
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- posiziono ev' (Y+T) come impostato sopra e V in posizione finale
|
|
dVPosA = dTPosA + dNewVDelta
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
end -- [B1Vs]
|
|
|
|
dYDeltaA = dYPosA - dTPosA
|
|
dVDeltaA = dVPosA - dTPosA
|
|
|
|
else -- **[B1Vns]** |spostamento| finale richiesto (ev' residuo) di |V non 'significativo'|
|
|
EmitComment( vCmd, '[B1Vns]')
|
|
if SpecTestOnlyRemarkInCmds( vCmd) then
|
|
-- risalita testa a Zmax
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
-- emetto posizione di V
|
|
table.insert( vCmd, { 1, 'V', dVPosA})
|
|
end
|
|
end -- [B1-s/ns]
|
|
|
|
-- aggiorno la verifica di movimento 'significativo' per Y
|
|
bYDeltaS = ( dYDeltaF < dYDeltaA - dYDeltaTol or dYDeltaF > dYDeltaA + DELTA_SIC)
|
|
-- **[B1Ys]** Se vi è uno |spostamento residuo di Y significativo'|
|
|
if bYDeltaS then
|
|
EmitComment( vCmd, '[B1Ys]')
|
|
-- risalita testa a Zmax
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
|
|
local dNewY = dTPosA + dNewYDelta
|
|
local dExtraY = dNewY - MyMinY
|
|
-- **[B1Ys-r]** se pos. NewY è raggiungibile direttamente
|
|
if dExtraY >= 0 then --dNewY > MyMinY then
|
|
EmitComment( vCmd, '[B1Ys-r]')
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- sposto il carrello Y
|
|
dYPosA = dTPosA + dNewYDelta
|
|
table.insert( vCmd, { 1, 'Y', dYPosA})
|
|
|
|
-- **[B1Ys-xn]** se pos. NewY non è raggiungibile direttamente, ma ExtraY non è 'significativo'
|
|
elseif -dExtraY <= dYDeltaTol / 2 then
|
|
EmitComment( vCmd, '[B1Ys-xn]')
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- accentro Y
|
|
dYPosA = MyMinY
|
|
table.insert( vCmd, { 1, 'Y', dYPosA})
|
|
-- [B1sY-nr.xs] ma ExtraY 'significativo'
|
|
|
|
else -- **[B1Ys-xs]**
|
|
EmitComment( vCmd, '[B1Ys-xs]')
|
|
EgtOutLog( ' CLAMP : caso [B1Ys-xs] non gestito')
|
|
error( ' CLAMP : caso [B1Ys-xs] non gestito')
|
|
|
|
end --[B1Ys-r/-nr]
|
|
else -- [B1Yns] spostamento residuo di Y non significativo
|
|
EmitComment( vCmd, '[B1Yns]')
|
|
end -- [B1Ys/ns]
|
|
|
|
-- calcolo i nuovi parametri di aggancio
|
|
dYDeltaA = dYPosA - dTPosA
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- se non emessi movimenti, imposto posizione V
|
|
if not bZmaxOk then
|
|
-- risalita testa a Zmax
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
table.insert( vCmd, { 1, 'V', dVPosA})
|
|
end
|
|
-- imposto stato carrelli, per eventuale uso pressori (sempre eseguita risalita Z)
|
|
table.insert( vCmd, { 11, CalcCharStatus( 'Y', dYDeltaA)})
|
|
table.insert( vCmd, { 12, CalcCharStatus( 'V', dVDeltaA)})
|
|
-- imposto i nuovi parametri di aggancio
|
|
table.insert( vCmd, { 21, dYDeltaA, dVDeltaA})
|
|
|
|
EgtOutLog( ' YDeltaA =' .. EgtNumToString( dYDeltaA) .. ' VDeltaA =' .. EgtNumToString( dVDeltaA), 1)
|
|
-- reset contatore
|
|
EMC.CNT = nil
|
|
|
|
SpecOutputCNT()
|
|
return vCmd
|
|
end -- SpecAdjustCarrB1
|
|
|
|
---------------------------------------------------------------------
|
|
-- *** [B2] da entrambi a entrambi i carrelli : Y+V -> Y+V ***
|
|
---------------------------------------------------------------------
|
|
function SpecAdjustCarrB2( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta)
|
|
EgtOutLog( ' *[B2] = Y+V -> Y+V', 1)
|
|
-- elenco comandi
|
|
local vCmd = {}
|
|
-- Commento
|
|
table.insert( vCmd, { 0, 'Y+V -> Y+V'})
|
|
-- se primo scambio
|
|
local MyMinY = EgtIf( EMC.CNT == 1, MinY + AGG_LOAD, MinY)
|
|
-- recupero le posizioni correnti
|
|
local dYPosA = dTPosI + dYDeltaI
|
|
local dVPosA = dTPosI + dVDeltaI
|
|
local dTPosA = dTPosI
|
|
local dYDeltaA = dYDeltaI
|
|
local dVDeltaA = dVDeltaI
|
|
local dNewYDelta
|
|
local dNewVDelta
|
|
-- incremento la distanza tra le due posizioni ( se abilitato e possibile)
|
|
local dYDeltaAgg, dVDeltaAgg = AdjustPositionsForB( dYDeltaF, dVDeltaF, EMC.TCING, EMC.HCING, EMC.HOVM, bFixedDelta)
|
|
-- tolleranze
|
|
local dYDeltaTol = dYDeltaAgg + GetDeltaTol( EMC.LB - dYDeltaF - dYDeltaAgg, EMC.TCING, EMC.HCING, EMC.HOVM, 'Y', bFixedDelta)
|
|
local dVDeltaTol = dVDeltaAgg + GetDeltaTol( dVDeltaF - dVDeltaAgg, EMC.TCING, EMC.HCING, EMC.HOVM, 'V', bFixedDelta)
|
|
-- definisco criteri per movimenti 'significativi' in base alle 'nuove' tolleranze
|
|
local bYDeltaS = ( dYDeltaF < dYDeltaA - dYDeltaTol or dYDeltaF > dYDeltaA + DELTA_SIC)
|
|
local bVDeltaS = ( dVDeltaF > dVDeltaA + dVDeltaTol or dVDeltaF < dVDeltaA - DELTA_SIC)
|
|
-- reimposto i delta finali in caso di spostamenti richiesti significativi, tenuto conto delle tolleranze
|
|
if bYDeltaS then
|
|
dNewYDelta = dYDeltaF + dYDeltaTol / 2
|
|
else
|
|
dNewYDelta = dYDeltaA
|
|
end
|
|
if bVDeltaS then
|
|
dNewVDelta = dVDeltaF - dVDeltaTol / 2
|
|
else
|
|
dNewVDelta = dVDeltaA
|
|
end
|
|
-- definisco 'ExtraY' e ExtraV'
|
|
local dNewY = dTPosA + dNewYDelta
|
|
local dNewV = dTPosA + dNewVDelta
|
|
local dExtraY = dNewY - MyMinY -- < 0 => nuova pos. di Y 'non raggiungibile'
|
|
local dExtraV = dNewV - MaxV -- > 0 => nuova pos. di V 'non raggiungibile'
|
|
local bYxs = -dExtraY > dYDeltaTol/2
|
|
local bVxs = dExtraV > dVDeltaTol/2
|
|
|
|
EgtOutLog( ' YDeltaI=' .. EgtNumToString( dYDeltaI) .. ' VDeltaI=' .. EgtNumToString( dVDeltaI) ..
|
|
' TPosI='.. EgtNumToString( dTPosI), 1)
|
|
EgtOutLog( ' YDeltaF=' .. EgtNumToString( dYDeltaF) .. ' VDeltaF=' .. EgtNumToString( dVDeltaF), 1)
|
|
EgtOutLog( ' NewYDelta=' .. EgtNumToString( dNewYDelta) .. ' NewVDelta=' .. EgtNumToString( dNewVDelta), 1)
|
|
-- flag risalita a Zmax
|
|
local bZmaxOk = false
|
|
|
|
-- **[B2V]** |accentramento di V|
|
|
if dVDeltaF > dVDeltaI then
|
|
EmitComment( vCmd, '[B2V]')
|
|
|
|
-- calcoli preliminari in caso di spostamento richiesto per V significativo
|
|
local dCorsaYfc, dCorsaVfc, dDistBkN, dCorsaYd, dCorsaVTd, dCorsaYTr, dCorsaVr, bXsw
|
|
if bVDeltaS then
|
|
-- eventuale risalita testa a Zmax
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
if bVxs then
|
|
-- calcolo le corse disponibili dei carrelli **dalle posizioni iniziali**
|
|
-- per allontanare Y e (V+T)
|
|
dCorsaYfc = MaxY - dYPosA
|
|
dCorsaVfc = dVPosA - MinV
|
|
dDistBkN = EMC.LB - dYDeltaA - MinJoin - EMC.TCING -- DistBack 'netta'
|
|
dCorsaYd, dCorsaVTd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistBkN, 'Bk')
|
|
-- calcolo le corse di 'recupero' accentrando (Y+T) e V dopo l'allontanamento
|
|
dCorsaYTr = dCorsaYd + (dYPosA - MyMinY)
|
|
dCorsaVr = dCorsaVTd -- ((MaxV-dNewV) non fa parte del recupero in quanto già disponibile!)
|
|
bXsw = bVDeltaS and bVxs and dExtraV > dCorsaYTr + dCorsaVr
|
|
end
|
|
end
|
|
|
|
-- **[B2V-xsw]** posizione NewV non raggiungibile, con |dEXtraV > CorsaVr + CorsaYTr|
|
|
-- e spostamento richiesto di V significativo;
|
|
while bXsw do
|
|
EmitComment( vCmd, '[B2V-xsw]')
|
|
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **1**: allontano Y e V(+T) quanto possibile
|
|
dYPosA = dYPosA + dCorsaYd
|
|
dVPosA = dVPosA - dCorsaVTd
|
|
dTPosA = dTPosA - dCorsaVTd
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- chiudo il carrello Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **2:** accentro (Y+T) e V
|
|
dYPosA = dYPosA - dCorsaYTr -- = MyMinY
|
|
dTPosA = dTPosA - dCorsaYTr
|
|
dVPosA = MaxV
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- valuto Delta ottenuti
|
|
dYDeltaA = dYPosA - dTPosA
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- aggiorno la verifica di spostamento significativo
|
|
bYDeltaS = ( dYDeltaF < dYDeltaA - dYDeltaTol or dYDeltaF > dYDeltaA + DELTA_SIC)
|
|
bVDeltaS = ( dVDeltaF > dVDeltaA + dVDeltaTol or dVDeltaF < dVDeltaA - DELTA_SIC)
|
|
-- aggiorno ExtraV residuo
|
|
dExtraV = dExtraV - dCorsaYTr - dCorsaVr
|
|
-- **aggiorno i recuperi disponibili** riallontanando Y e (V+T) e riaccentrando (Y+T) e V
|
|
dCorsaYfc = dCorsaY -- MaxY - dNewY
|
|
dCorsaVfc = dCorsaV -- dNewV - MinV
|
|
dDistBkN = EMC.LB - dYDeltaA - MinJoin - EMC.TCING -- DistBack 'netta'
|
|
dCorsaYd, dCorsaVTd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistBkN, 'Bk')
|
|
dCorsaYTr = dCorsaYd
|
|
dCorsaVr = dCorsaVTd
|
|
-- aggiorno verifica per ripetizione del ciclo
|
|
bXsw = bVDeltaS and bVxs and dExtraV > dCorsaYTr + dCorsaVr
|
|
end -- [B2V-xsw]
|
|
|
|
|
|
--[B2Vs] |accentramento| (ev' residuo) |di V 'significativo'|
|
|
if bVDeltaS then
|
|
--[B2Vs-x] accentramento di V 'significativo' |con pos. NewV 'non raggiungibile'|(= oltre MaxV)
|
|
if dExtraV > 0 then
|
|
-- (ExtraV <= (CorsaYTr+CorsaVr) da ciclo precedente)
|
|
--[B2Vs-xs] accentramento di V con |ExtraV 'significativo'|
|
|
if bVxs then
|
|
local dCorsaYTrA = dYPosA - MyMinY
|
|
local dCorsaVra = MaxV - dVPosA
|
|
-- **[B2Vs-xs1]** se posso recuperare ExtraV solo accentrando V e (Y+T)
|
|
if dCorsaYTrA >= dExtraV then
|
|
EmitComment( vCmd, '[B2Vs-xs1]')
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **1:** accentro (Y+T) q.b. e V
|
|
dYPosA = dYPosA - dExtraV
|
|
dTPosA = dTPosA - dExtraV
|
|
dVPosA = MaxV -- (pos. finale)
|
|
dExtraV = 0
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
|
|
else -- **[B2Vs-xs2]**
|
|
EmitComment( vCmd, '[B2Vs-xs2]')
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **1: allontano** (V+T) e Y
|
|
-- tenendo conto di dover 'recuperare' ExtraV...
|
|
local dYPos1, dVPos1, dTPos1 = PosXs2Enl( dYPosA, dVPosA, dTPosA, dExtraV, dCorsaYTrA, dCorsaYd, dCorsaVTd, 'V')
|
|
local dYDispl1 = dYPos1 - dYPosA
|
|
local dVDispl1 = dVPos1 - dVPosA
|
|
--local dTDispl1 = dTPos1 - dTPosA
|
|
local dYDeltaDiff = dNewYDelta - (dYPos1 - dTPos1)
|
|
-- ...e anche possibilmente di posizionare Y alla posizione finale
|
|
if dYDeltaDiff > 0 then
|
|
dCorsaYfc = MaxY - dYPos1
|
|
dCorsaVfc = dVPos1 - MinV
|
|
dDistBkN = EMC.LB - (dYPos1-dTPos1) - MinJoin - EMC.TCING -- DistBack1 'netta'
|
|
dCorsaYd, dCorsaVTd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistBkN, 'Bk')
|
|
--dYPosA, dVPosA, dTPosA = PosXs2Enl( dYPos1, dVPos1, dTPos1, dYDeltaDiff, 0, dCorsaYd-dYDispl1, dCorsaVTd+dVDispl1, 'V')
|
|
dYPosA, dVPosA, dTPosA = PosXs2Enl( dYPos1, dVPos1, dTPos1, dYDeltaDiff, 0, dCorsaYd, dCorsaVTd, 'V')
|
|
else
|
|
dYPosA = dYPos1; dVPosA = dVPos1; dTPosA = dTPos1
|
|
end
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA })
|
|
-- chiudo il carrello Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **2: accentro** (Y+T) e V
|
|
dYPosA = dYPosA - dYDispl1 - dCorsaYTrA -- POS. FINALE
|
|
dTPosA = dTPosA - dYDispl1 - dCorsaYTrA
|
|
dVPosA = dVPosA - dVDispl1 + dCorsaVra -- POS. FINALE
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
end -- [B2Vs-xs1/xs2]
|
|
|
|
-- **[B2Vs-xn]** |dExtraV non 'significativo'|
|
|
else
|
|
EmitComment( vCmd, '[B2Vs-xn]')
|
|
dExtraV = 0
|
|
-- ev' chiudo Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- 1: posiziono V assumendo maxV come pos. finale
|
|
dVPosA = MaxV -- (pos finale)
|
|
table.insert( vCmd, { 1, 'V', dVPosA})
|
|
end -- [B2Vs-xs/xn]
|
|
|
|
-- **[B2Vs-r]** accentramento di |V| 'significativo' con pos.|'raggiungibile'| (= non oltre MaxV)
|
|
else
|
|
EmitComment( vCmd, '[B2Vs-r]')
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- 1: posiziono V -- (pos finale)
|
|
dVPosA = dTPosA + dNewVDelta
|
|
dExtraV = 0
|
|
table.insert( vCmd, { 1, 'V', dVPosA})
|
|
end --[B2Vs]
|
|
|
|
-- **[B2Vns]** accentramento di V non 'significativo'|
|
|
else
|
|
EmitComment( vCmd, '[B2Vns]')
|
|
dExtraV = 0
|
|
end --[B2Vs/ns]
|
|
end --[B2V] ( accentramento di V)
|
|
|
|
-- ---------------------------------------------------------------------------
|
|
-- **[B2Y]** |accentramento di Y|
|
|
if dYDeltaF < dYDeltaI then
|
|
EmitComment( vCmd, '[B2Y]')
|
|
|
|
-- calcoli preliminari in caso di spostamento richiesto per Y significativo
|
|
local dCorsaYfc, dCorsaVfc, dDistFrN, dCorsaYTd, dCorsaVd, dCorsaVTr, dCorsaYr
|
|
if bYDeltaS then
|
|
-- eventuale risalita testa a Zmax
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
if bYxs then
|
|
-- calcolo le **corse disponibili** dei carrelli dalle posizioni iniziali
|
|
-- per allontanare (Y+T) e V
|
|
dCorsaYfc = MaxY - dYPosA
|
|
dCorsaVfc = dVPosA - MinV
|
|
dDistFrN = dVDeltaA - MinJoin - EMC.HCING - EMC.HOVM -- DistFront 'netta'
|
|
dCorsaYTd, dCorsaVd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistFrN, 'Fr')
|
|
-- valuto le corse di 'recupero' dai due carrelli possibili riaccentrando Y e (V+T)
|
|
dCorsaVTr = dCorsaVd + (MaxV - dVPosA)
|
|
dCorsaYr = dCorsaYTd
|
|
bXsw = bYDeltaS and bYxs and -dExtraY > dCorsaVTr + dCorsaYr
|
|
end
|
|
end
|
|
|
|
-- **[B2Ys-xsw]** |-dEXtraY > dCorsaYr + dCorsaVTr|
|
|
while bXsw do
|
|
EmitComment( vCmd, '[B2Y-xsw]')
|
|
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- |1:| allontano (Y+T) e V quanto possibile
|
|
dYPosA = dYPosA + dCorsaYTd
|
|
dTPosA = dTPosA + dCorsaYTd
|
|
dVPosA = dVPosA - dCorsaVd
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- chiudo il carrello V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
|
|
-- |2:| accentro Y e (V+T)
|
|
dYPosA = MyMinY -- = dYPosA - dCorsaYr
|
|
dVPosA = dVPosA + dCorsaVTr -- = MaxV
|
|
dTPosA = dTPosA + dCorsaVTr
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- valuto Delta 'attuali'
|
|
dYDeltaA = dYPosA - dTPosA
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- aggiorno la verifica di spostamento significativo
|
|
bYDeltaS = ( dYDeltaF < dYDeltaA - dYDeltaTol or dYDeltaF > dYDeltaA + DELTA_SIC)
|
|
bVDeltaS = ( dVDeltaF > dVDeltaA + dVDeltaTol or dVDeltaF < dVDeltaA - DELTA_SIC)
|
|
-- aggiorno ExtraY residuo
|
|
dExtraY = dExtraY + dCorsaYr + dCorsaVTr
|
|
-- aggiorno i recuperi disponibili riallontanando (Y+T) V e riaccentrando succ' Y e (V+T)
|
|
dCorsaYfc = dCorsaY -- MaxY - dNewY
|
|
dCorsaVfc = dCorsaV -- dNewV - MinV
|
|
dDistFrN = dVDeltaA - MinJoin - EMC.HCING - EMC.HOVM -- DistFront 'netta'
|
|
dCorsaYTd, dCorsaVd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistFrN, 'Fr')
|
|
-- valuto le corse di 'recupero' dai due carrelli possibili riaccentrando Y e (V+T)
|
|
dCorsaVTr = dCorsaVd
|
|
dCorsaYr = dCorsaYTd
|
|
-- aggiorno verifica per ripetizione del ciclo
|
|
bXsw = bYDeltaS and bYxs and -dExtraY > dCorsaVTr + dCorsaYr
|
|
end -- [B2Ys-xsw]
|
|
|
|
-- **[B2Ys]** |accentramento di Y 'significativo'|
|
|
if bYDeltaS then
|
|
-- **[B2Ys-x]** accentramento di Y 'significativo' |con pos. NewY 'non raggiungibile'|(= oltre MyMinY)
|
|
if dExtraY < 0 then
|
|
-- (-ExtraY <= (dCorsaYr + dCorsaVTr ) a ciclo precedente
|
|
-- **[B2Ys-xs]** accentramento di Y con |'ExtraY 'significativo'|
|
|
if bYxs then
|
|
local dCorsaVTrA = MaxV - dVPosA
|
|
local dCorsaYra = dYPosA - MyMinY
|
|
-- **[B2Ys-xs1]** se posso recuperare ExtraY solo accentrando Y e (V+T)
|
|
if dCorsaVTrA >= -dExtraY then
|
|
EmitComment( vCmd, '[B2Ys-xs1]')
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- accentro Y e (V+T) q.b.
|
|
dYPosA = MyMinY -- (pos. finale)
|
|
dVPosA = dVPosA - dExtraY -- = +(-dEXtraY)
|
|
dTPosA = dTPosA - dExtraY -- = +(-dEXtraY)
|
|
dExtraY = 0
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
|
|
else -- **[B2Ys-xs2]**
|
|
EmitComment( vCmd, '[B2Ys-xs2]')
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **1: allontano** (Y+T) e V
|
|
-- tenendo conto di dover 'recuperare' ExtraY...
|
|
dYPos1, dVPos1, dTPos1 = PosXs2Enl( dYPosA, dVPosA, dTPosA, dExtraY, dCorsaVTrA, dCorsaYTd, dCorsaVd, 'Y')
|
|
local dYDispl1 = dYPos1 - dYPosA
|
|
local dVDispl1 = dVPos1 - dVPosA
|
|
local dVDeltaDiff = dNewVDelta - (dVPos1 - dTPos1)
|
|
-- ...e anche possibilmente di posizionare V alla posizione finale
|
|
if dVDeltaDiff < 0 then
|
|
dCorsaYfc = MaxY - dYPos1
|
|
dCorsaVfc = dVPos1 - MinV
|
|
dDistFrN = dVDeltaA - MinJoin - EMC.HCING - EMC.HOVM -- DistFront1 'netta'
|
|
dCorsaYTd, dCorsaVd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistFrN, 'Fr')
|
|
dYPosA, dVPosA, dTPosA = PosXs2Enl( dYPos1, dVPos1, dTPos1, dVDeltaDiff, 0, dCorsaYTd, dCorsaVd, 'Y')
|
|
else
|
|
dYPosA = dYPos1; dVPosA = dVPos1; dTPosA = dTPos1
|
|
end
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA })
|
|
--dVDeltaA = dVPosA - dTPosA
|
|
-- chiudo il carrello V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- |2:| accentro Y e (V+T)
|
|
dYPosA = dYPosA - dYDispl1 - dCorsaYra -- POS. FINALE
|
|
dVPosA = dVPosA - dVDispl1 + dCorsaVTrA -- POS. FINALE
|
|
dTPosA = dTPosA - dVDispl1 + dCorsaVTrA
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
end -- [B2Ys-xs1/xs2]
|
|
|
|
-- **[B2Ys-xn]** |dExtraY non 'significativo'|
|
|
else
|
|
EmitComment( vCmd, '[B2Ys-xn]')
|
|
dExtraY = 0
|
|
-- ev' chiudo V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- 1: posiziono Y assumendo MyMinY come pos. finale
|
|
dYPosA = MyMinY -- (pos finale)
|
|
table.insert( vCmd, { 1, 'Y', dYPosA})
|
|
end -- [B2Ys-xs/xn]
|
|
|
|
--[B2Ys-r] accentramento di |Y| 'significativo' con pos.|'raggiungibile'| (non oltre MyMinY)
|
|
else
|
|
EmitComment( vCmd, '[B2Ys-r]')
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- |1:| posiziono Y -- (pos finale)
|
|
dYPosA = dTPosA + dNewYDelta
|
|
dExtraV = 0
|
|
table.insert( vCmd, { 1, 'Y', dYPosA})
|
|
end --[B2Ys]
|
|
|
|
-- [B2Yns] accentramento di V non 'significativo'|
|
|
else
|
|
EmitComment( vCmd, '[B2Yns]')
|
|
dExtraV = 0
|
|
end --[B2Ys/ns]
|
|
end --[B2Y] ( accentramento di V)
|
|
|
|
-- ricalcolo gli 'spostamenti significativi'
|
|
dYDeltaA = dYPosA - dTPosA
|
|
dVDeltaA = dVPosA - dTPosA
|
|
bYDeltaS = ( dYDeltaF < dYDeltaA - dYDeltaTol or dYDeltaF > dYDeltaA + DELTA_SIC)
|
|
bVDeltaS = ( dVDeltaF > dVDeltaA + dVDeltaTol or dVDeltaF < dVDeltaA - DELTA_SIC)
|
|
|
|
-- ------------------------------------------------------------------------------------------
|
|
-- **[B2C]** |Spostamenti| di Y e/o V (ev' |residui| da cicli precedenti)
|
|
-- ++ possono essere solo allontanamenti ? +++++++++++++++++++++
|
|
EmitComment( vCmd, '[B2C]')
|
|
-- **[B2Cs]** se c'è uno |spostamento (ev' residuo) significativo|
|
|
if bYDeltaS or bVDeltaS then
|
|
-- **[B2CYs]** se lo |spostamento (ev' residuo) di Y è 'significativo'|
|
|
if bYDeltaS then
|
|
EmitComment( vCmd, '[B2CYs]')
|
|
-- eventuale risalita testa a Zmax
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
|
|
local dNewY = dTPosA + dNewYDelta
|
|
local dExtraY = dNewY - MyMinY
|
|
|
|
-- **[B2CYs-r]** se posizione di |Y raggiungibile|
|
|
if dExtraY >= 0 and dNewY <= MaxY then
|
|
EmitComment( vCmd, '[B2CYs-r]')
|
|
-- chiudo eventualmente V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- sposto il carrello Y
|
|
dYPosA = dNewY
|
|
table.insert( vCmd, { 1, 'Y', dYPosA})
|
|
-- **[B2CYs-nr]**
|
|
else
|
|
EmitComment( vCmd, '[B2CYs-nr]')
|
|
EgtOutLog( ' CLAMP : posizione Y non raggiungibile' .. '- caso [B2Ys-nr] non gestito')
|
|
error( 'CLAMP : posizione Y non raggiungibile')
|
|
end
|
|
end -- [B2CYs]
|
|
|
|
-- **[B2CVs]** se lo |spostamento (ev' residuo) di V è 'significativo'|
|
|
if bVDeltaS then
|
|
EmitComment( vCmd, '[B2CVs]')
|
|
-- eventuale risalita testa a Zmax
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
|
|
local dNewV = dTPosA + dNewVDelta
|
|
local dExtraV = dNewV - MaxV
|
|
|
|
-- **[B2CVs-r]** se posizione NewV raggiungibile
|
|
if dExtraV <= 0 and dNewV >= MinV then
|
|
EmitComment( vCmd, '[B2CVs-r]')
|
|
-- chiudo eventualmente Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- sposto il carrello V
|
|
dVPosA = dNewV
|
|
table.insert( vCmd, { 1, 'V', dVPosA})
|
|
-- **[B2CVs-nr]**
|
|
else -- se ExtraV > 0 anche se non significativo
|
|
EmitComment( vCmd, '[B2CVs-nr]')
|
|
EgtOutLog( ' CLAMP : posizione V non raggiungibile' .. '- caso [B2cVs-nr] non gestito')
|
|
error( 'CLAMP : posizione V non raggiungibile')
|
|
end
|
|
end --[B2CsV]
|
|
|
|
-- **[B2Cn]** |nessuno spostamento (ev' residuo significativo)|
|
|
else
|
|
EmitComment( vCmd, '[B2Cn]')
|
|
end -- [B2C]
|
|
|
|
-- calcolo i nuovi parametri di aggancio
|
|
dYDeltaA = dYPosA - dTPosA
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- imposto stato carrelli, per eventuale uso pressori
|
|
table.insert( vCmd, { 11, CalcCharStatus( 'Y', dYDeltaA)})
|
|
table.insert( vCmd, { 12, CalcCharStatus( 'V', dVDeltaA)})
|
|
-- imposto i nuovi parametri di aggancio
|
|
table.insert( vCmd, { 21, dYDeltaA, dVDeltaA})
|
|
|
|
EgtOutLog( ' YDeltaA =' .. EgtNumToString( dYDeltaA) .. ' VDeltaA =' .. EgtNumToString( dVDeltaA), 1)
|
|
-- reset contatore
|
|
EMC.CNT = nil
|
|
|
|
SpecOutputCNT()
|
|
return vCmd
|
|
end --SpecAdjustCarrB2
|
|
|
|
---------------------------------------------------------------------------------------------------------
|
|
-- *** [B3] da carrello V a entrambi i carrelli : V -> Y+V ***
|
|
---------------------------------------------------------------------------------------------------------
|
|
function SpecAdjustCarrB3( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta)
|
|
EgtOutLog( ' *[B3] = V -> Y+V', 1)
|
|
-- elenco comandi
|
|
local vCmd = {}
|
|
-- Commento
|
|
table.insert( vCmd, { 0, 'V -> Y+V'})
|
|
-- se primo scambio
|
|
local MyMinY = EgtIf( EMC.CNT == 1, MinY + AGG_LOAD, MinY)
|
|
local dCorsaY = MaxY - MyMinY
|
|
local dCorsaV = MaxV - MinV
|
|
-- recupero le posizioni correnti
|
|
local dVPosA = dTPosI + dVDeltaI
|
|
local dYPosA = ParkY
|
|
local dTPosA = dTPosI
|
|
local dVDeltaA = dVDeltaI
|
|
local dYDeltaA = dYPosA - dTPosA
|
|
local dNewYDelta
|
|
local dNewVDelta
|
|
-- incremento la distanza tra le due posizioni ( se abilitato e possibile)
|
|
local dYDeltaAgg, dVDeltaAgg = AdjustPositionsForB( dYDeltaF, dVDeltaF, EMC.TCING, EMC.HCING, EMC.HOVM, bFixedDelta)
|
|
-- tolleranze
|
|
local dYDeltaTol = dYDeltaAgg + GetDeltaTol( EMC.LB - dYDeltaF - dYDeltaAgg, EMC.TCING, EMC.HCING, EMC.HOVM, 'Y', bFixedDelta)
|
|
local dVDeltaTol = dVDeltaAgg + GetDeltaTol( dVDeltaF - dVDeltaAgg, EMC.TCING, EMC.HCING, EMC.HOVM, 'V', bFixedDelta)
|
|
-- definisco criteri per movimenti 'significativi' in base alle 'nuove' tolleranze
|
|
local bYDeltaS = ( dYDeltaF < dYDeltaA - dYDeltaTol or dYDeltaF > dYDeltaA + DELTA_SIC)
|
|
local bVDeltaS = ( dVDeltaF > dVDeltaA + dVDeltaTol or dVDeltaF < dVDeltaA - DELTA_SIC)
|
|
-- reimposto i delta finali in caso di spostamenti richiesti significativi, tenuto conto delle tolleranze
|
|
if bYDeltaS then
|
|
dNewYDelta = dYDeltaF + dYDeltaTol / 2
|
|
else
|
|
dNewYDelta = dYDeltaA
|
|
end
|
|
if bVDeltaS then
|
|
dNewVDelta = dVDeltaF - dVDeltaTol / 2
|
|
else
|
|
dNewVDelta = dVDeltaA
|
|
end
|
|
|
|
EgtOutLog( ' YDeltaI(Park)='.. EgtNumToString( dYDeltaA)..' VDeltaI=' .. EgtNumToString( dVDeltaA) ..
|
|
' TPosI='.. EgtNumToString( dTPosI), 1)
|
|
EgtOutLog( ' YDeltaF=' .. EgtNumToString( dYDeltaF) .. ' VDeltaF=' .. EgtNumToString( dVDeltaF), 1)
|
|
EgtOutLog( ' NewYDelta=' .. EgtNumToString( dNewYDelta) .. ' NewVDelta=' .. EgtNumToString( dNewVDelta), 1)
|
|
-- risalita testa a Zmax
|
|
local bZmaxOk = false
|
|
|
|
-- calcoli preliminari in caso di spostamento richiesto per Y significativo
|
|
local dYPos, dVPos, dTPos
|
|
local dNewY, dExtraY, bYxs
|
|
local dCorsaYfc, dCorsaVfc, dDistFrN, dCorsaYTd, dCorsaVd, dCorsaVTr, dCorsaYr, bXsw
|
|
if bYDeltaS then
|
|
-- definisco 'ExtraY' con (V+T) e Y accentrati q.b. per la presa di Y
|
|
dYPos, dVPos, dTPos = PosxExtraYV( dYPosA, dVPosA, dTPosA, MyMinY, MaxV, 'Y')
|
|
dNewY = dTPos + dNewYDelta
|
|
dExtraY = dNewY - MyMinY -- se < 0 pos. Y non direttamente 'raggiungibile'
|
|
bYxs = -dExtraY > dYDeltaTol/2 -- ExtraY 'significativo'
|
|
if bYxs then
|
|
-- calcolo le **corse disponibili dei carrelli a partire da Y e V c.s.** per allontanare q.p. (Y+T) e V
|
|
dCorsaYfc = MaxY - dYPos
|
|
dCorsaVfc = dVPos - MinV
|
|
dDistFrN = dVDeltaA - MinJoin - EMC.HCING - EMC.HOVM -- DistFront 'netta'
|
|
dCorsaYTd, dCorsaVd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistFrN, 'Fr')
|
|
-- valuto **le corse di 'recupero'** dai due carrelli possibili riaccentrando Y e (V+T)
|
|
dCorsaVTr = dCorsaVd + (MaxV - dVPos)
|
|
dCorsaYr = dCorsaYTd
|
|
bXsw = bYDeltaS and bYxs and -dExtraY > dCorsaVTr + dCorsaYr
|
|
end
|
|
-- inizializzo
|
|
dYPosA = dYPos
|
|
dVPosA = dVPos
|
|
dTPosA = dTPos
|
|
end
|
|
|
|
-- **[B3Ys-xsw]** posizione finale dNewV non raggiungibile, con |dEXtraY > CorsaVr + CorsaYr|
|
|
while bXsw do
|
|
EmitComment( vCmd, '[B3Ys-xsw]')
|
|
|
|
-- risalita testa a Zmax
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
|
|
-- |1:| posiziono (V+T) e Y come calcolato sopra
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- |2:| allontano (Y+T) e V quanto possibile
|
|
dYPosA = dYPosA + dCorsaYTd
|
|
dTPosA = dTPosA + dCorsaYTd
|
|
dVPosA = dVPosA - dCorsaVd
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- chiudo il carrello V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- |3:| accentro (V+T) e Y
|
|
dVPosA = dVPosA + dCorsaVTr
|
|
dTPosA = dTPosA + dCorsaVTr
|
|
dYPosA = MyMinY
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- valuto i Delta ottenuti
|
|
dYDeltaA = dYPosA - dTPosA
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- aggiorno la verifica di spostamento significativo
|
|
bYDeltaS = ( dYDeltaF < dYDeltaA - dYDeltaTol or dYDeltaF > dYDeltaA + DELTA_SIC)
|
|
bVDeltaS = ( dVDeltaF > dVDeltaA + dVDeltaTol or dVDeltaF < dVDeltaA - DELTA_SIC)
|
|
-- aggiorno ExtraY
|
|
dExtraY = dExtraY + dCorsaYr + dCorsaVTr
|
|
|
|
-- **aggiorno la valutazione delle corse disponibili**
|
|
-- a partire da Y e V c.s. per allontanare q.p. (Y+T) e V
|
|
dCorsaYfc = MaxY - dYPosA
|
|
dCorsaVfc = dVPosA - MinV
|
|
dDistFrN = dVDeltaA - MinJoin - EMC.HCING - EMC.HOVM -- DistFront 'netta'
|
|
dCorsaYTd, dCorsaVd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistFrN, 'Fr')
|
|
-- valuto le corse di 'recupero' dai due carrelli possibili riaccentrando Y e (V+T)
|
|
dCorsaVTr = dCorsaVd
|
|
dCorsaYr = dCorsaYTd
|
|
-- aggiorno verifica per ripetizione del ciclo
|
|
bXsw = bYDeltaS and bYxs and -dExtraY > dCorsaVTr + dCorsaYr
|
|
end --[B3Ys-xsw]
|
|
|
|
-- **[B3Ys]** |spostamento| finale richiesto (ev' residuo) di |Y| |'significativo'|
|
|
if bYDeltaS then
|
|
EmitComment( vCmd, '[B3Ys]')
|
|
|
|
-- risalita testa a Zmax
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
|
|
-- **[B3Ys-x]** posizione di |Y non raggiungibile|
|
|
-- ( -dExtraY <= (CorsaYr+CorsaVr) da ciclo precedente )
|
|
if dExtraY < 0 then
|
|
--EmitComment( vCmd, '[B3Ys-x]')
|
|
|
|
-- **[B3Ys-xs]** |ExtraY 'significativo'|
|
|
if bYxs then
|
|
--EmitComment( vCmd, '[B3Ys-xs]')
|
|
|
|
local dCorsaVTrA = MaxV - dVPosA
|
|
local dCorsaYra = dYPosA - MyMinY
|
|
-- **[B3Ys-xs1]** se posso recuperare ExtraY semplicem' accentrando Y e (V+T)
|
|
-- ulteriormente rispetto a YPos e Vpos definiti c.s.
|
|
if dCorsaVTrA >= -dExtraY then
|
|
EmitComment( vCmd, '[B3Ys-xs1]')
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **1: accentro** Y e (V+T)
|
|
dYPosA = MyMinY -- (pos. finale)
|
|
dVPosA = dVPosA - dExtraY
|
|
dTPosA = dTPosA - dExtraY
|
|
dYDeltaA = dYPosA - dTPosA
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
|
|
else -- **[B3Ys-xs2]**
|
|
EmitComment( vCmd, '[B3Ys-xs2]')
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **1: accentro** ev' Y e (V+T) alle posizioni impostate
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
dYDeltaA = dYPosA - dTPosA
|
|
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **2: allontano** (Y+T) e V
|
|
-- tenendo conto di dover 'recuperare' ExtraY...
|
|
dYPos1, dVPos1, dTPos1 = PosXs2Enl( dYPosA, dVPosA, dTPosA, dExtraY, dCorsaVTrA, dCorsaYTd, dCorsaVd, 'Y')
|
|
local dYDispl1 = dYPos1 - dYPosA
|
|
local dVDispl1 = dVPos1 - dVPosA
|
|
local dVDeltaDiff = dNewVDelta - (dVPos1 - dTPos1)
|
|
-- ...e anche di posizionare possibilmente V alla posizione finale
|
|
if dVDeltaDiff < 0 then
|
|
CorsaYfc = MaxY - dYPos1
|
|
dCorsaVfc = dVPos1 - MinV
|
|
dDistFrN = dVDeltaA - MinJoin - EMC.HCING - EMC.HOVM -- DistFront1 'netta'
|
|
dCorsaYTd, dCorsaVd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistFrN, 'Fr')
|
|
dYPosA, dVPosA, dTPosA = PosXs2Enl( dYPos1, dVPos1, dTPos1, dVDeltaDiff, 0, dCorsaYTd, dCorsaVd, 'Y')
|
|
else
|
|
dYPosA = dYPos1; dVPosA = dVPos1; dTPosA = dTPos1
|
|
end
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA })
|
|
|
|
-- chiudo il carrello V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **3: accentro** Y e (V+T)
|
|
dYPosA = dYPosA - dYDispl1 - dCorsaYra -- POS. FINALE
|
|
dVPosA = dVPosA - dVDispl1 + dCorsaVTrA -- POS. FINALE
|
|
dTPosA = dTPosA - dVDispl1 + dCorsaVTrA
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
end -- [B3Ys-xs1/-xs2]
|
|
|
|
-- **[B3Ys-xn]** |dExtraY ancora < 0 ma non 'significativo')|
|
|
else --if -dExtraY < DeltaToll/2
|
|
EmitComment( vCmd, '[B3Ys-xn]')
|
|
-- ev' chiudo V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- 1: accentro Y e porto (V+T) alla posizione impostata
|
|
dExtraY = 0
|
|
dYPosA = MyMinY -- (pos. finale)
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
end --[B3Ys-xs/xn]
|
|
|
|
-- **[B3Ys-r]** |dExtraY > 0 (pos. Y raggiungibile)|
|
|
else
|
|
EmitComment( vCmd, '[B3Ys-r]')
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- posizione ev' (V+T) come impostato sopra e Y in posizione finale
|
|
dYPosA = dTPosA + dNewYDelta
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
end -- [B3Ys]
|
|
--
|
|
dYDeltaA = dYPosA - dTPosA
|
|
dVDeltaA = dVPosA - dTPosA
|
|
|
|
else -- [B3Yns] |spostamento| finale richiesto (ev' residuo) di |Y non 'significativo'|
|
|
EmitComment( vCmd, '[B3Yns]')
|
|
end --[B3Ys/ns]
|
|
|
|
-- aggiorno la verifica di movimento 'significativo' per V
|
|
bVDeltaS = ( dVDeltaF > dVDeltaA + dVDeltaTol or dVDeltaF < dVDeltaA - DELTA_SIC)
|
|
-- **[B3Vs]** Se vi è uno |spostamento residuo di V significativo'|
|
|
if bVDeltaS then
|
|
EmitComment( vCmd, '[B3Vs]')
|
|
|
|
-- risalita testa a Zmax
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
|
|
local dNewV = dTPosA + dNewVDelta
|
|
local dExtraV = dNewV - MaxV
|
|
|
|
-- **[B3Vs-x]** se pos. |NewV non è raggiungibile direttamente| (oltre MaxV)
|
|
if dExtraV > 0 then --dNewV <= MaxV then
|
|
-- **[B3Vs-xs]** |ExtraV 'significativo'|
|
|
if dExtraV > dVDeltaTol /2 then
|
|
EmitComment( vCmd, '[B3Vs-xs]')
|
|
EgtOutLog( ' CLAMP : caso [B3Vs-xs] non gestito')
|
|
error( ' CLAMP : caso [B3Vs-xs] non gestito')
|
|
|
|
-- **[B3Vs-xn]** |ExtraV non 'significativo'|
|
|
else
|
|
EmitComment( vCmd, '[B3Vs-xn]')
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- accentro V q.p.
|
|
dVPosA = MaxV
|
|
table.insert( vCmd, { 1, 'V', dVPosA})
|
|
end -- [B3Vs-xs/xn]
|
|
|
|
else -- **[B3Vs-r]** pos. |NewV raggiungibile direttamente| (non oltre MaxV)
|
|
EmitComment( vCmd, '[B3Vs-r]')
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- sposto il carrello V
|
|
dVPosA = dNewV
|
|
table.insert( vCmd, { 1, 'V', dVPosA})
|
|
end --[B3Vs-x/-r]
|
|
|
|
else -- [B3Vns] spostamento residuo di V non significativo
|
|
EmitComment( vCmd, '[B3Vns]')
|
|
end -- [B3Vs/ns]
|
|
|
|
-- calcolo i nuovi parametri di aggancio
|
|
dYDeltaA = dYPosA - dTPosA
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- se non emessi movimenti, imposto posizione Y
|
|
if not bZmaxOk then
|
|
-- risalita testa a Zmax
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
table.insert( vCmd, { 1, 'Y', dYPosA})
|
|
end
|
|
-- imposto stato carrelli, per eventuale uso pressori (sempre effettuato movimento)
|
|
table.insert( vCmd, { 11, CalcCharStatus( 'Y', dYDeltaA)})
|
|
table.insert( vCmd, { 12, CalcCharStatus( 'V', dVDeltaA)})
|
|
-- imposto i nuovi parametri di aggancio
|
|
table.insert( vCmd, { 21, dYDeltaA, dVDeltaA})
|
|
|
|
EgtOutLog( ' YDeltaA =' .. EgtNumToString( dYDeltaA) .. ' VDeltaA =' .. EgtNumToString( dVDeltaA), 1)
|
|
-- reset contatore
|
|
EMC.CNT = nil
|
|
|
|
SpecOutputCNT()
|
|
return vCmd
|
|
end -- SpecAdjustCarrB3
|
|
|
|
---------------------------------------------------------------------
|
|
-- *** [C1] da carrello Y a V : Y -> V ***
|
|
---------------------------------------------------------------------
|
|
function SpecAdjustCarrC1( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta)
|
|
EgtOutLog( ' *[C1] = Y -> V ', 1)
|
|
-- elenco comandi
|
|
local vCmd = {}
|
|
-- Commento
|
|
table.insert( vCmd, { 0, 'Y -> V'})
|
|
-- se primo scambio
|
|
local MyMinY = EgtIf( EMC.CNT == 1, MinY + AGG_LOAD, MinY)
|
|
-- recupero le posizioni correnti
|
|
--local dCorsaY = MaxY - MyMinY
|
|
--local dCorsaV = MaxV - MinV
|
|
local dYPosA = dTPosI + dYDeltaI
|
|
local dVPosA = ParkV
|
|
local dTPosA = dTPosI
|
|
local dYDeltaA = dYDeltaI
|
|
local dVDeltaA = dVPosA - dTPosA
|
|
local dNewVDelta -- = dVDeltaF
|
|
-- tolleranza
|
|
local dVDeltaTol = GetDeltaTol( dVDeltaF, EMC.TCING, EMC.HCING, EMC.HOVM, 'V', bFixedDelta)
|
|
local bVDeltaS = ( dVDeltaF > dVDeltaA + dVDeltaTol or dVDeltaF < dVDeltaA - DELTA_SIC)
|
|
-- reimposto i delta finali in caso di spostamenti richiesti significativi, tenuto conto delle tolleranze
|
|
if bVDeltaS then
|
|
dNewVDelta = dVDeltaF - dVDeltaTol/2
|
|
else
|
|
dNewVDelta = dVDeltaA
|
|
end
|
|
EgtOutLog(' YDeltaI=' .. EgtNumToString( dYDeltaA) .. ' VDeltaI(Park)='.. EgtNumToString( dVDeltaA)..
|
|
' TPosI=' .. EgtNumToString( dTPosI) , 1)
|
|
EgtOutLog(' VDeltaF=' .. EgtNumToString( dVDeltaF) .. ' NewVDelta=' .. EgtNumToString( dNewVDelta) , 1)
|
|
|
|
-- risalita testa a Zmax (da effettuare comunque, dato lo scambio di carrelli)
|
|
local bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
|
|
-- calcoli preliminari in caso di spostamento richiesto per V significativo
|
|
local dYPos, dVPos, dTPos
|
|
local dNewV, dExtraV, bVxs
|
|
local dCorsaYfc, dCorsaVfc, dDistFrN, dCorsaYTd, dCorsaVd, dCorsaVTr, dCorsaYr, bXsw
|
|
if bVDeltaS then
|
|
-- definisco 'ExtraV' con (Y+T) e V accentrati q.b. per la presa di V
|
|
dYPos, dVPos, dTPos = PosxExtraYV( dYPosA, dVPosA, dTPosA, MyMinY, MaxV, 'V')
|
|
dNewV = dTPos + dNewVDelta
|
|
dExtraV = dNewV - MaxV
|
|
bVxs = dExtraV > dVDeltaTol/2 -- ExtraV 'significativo'
|
|
if bVxs then
|
|
-- calcolo le **corse disponibili dei carrelli a partire da Y e V c.s.** per allontanare q.p. Y e (V+T)
|
|
dCorsaYfc = MaxY - dYPos
|
|
dCorsaVfc = dVPos - MinV
|
|
--local dDistBkN = EMC.LB - dYDeltaA - MinJoin - EMC.TCING -- DistBack 'netta'
|
|
dDistBkN = EMC.LB - (dYPos-dTPos) - MinJoin - EMC.TCING -- DistBack 'netta'
|
|
dCorsaYd, dCorsaVTd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistBkN, 'Bk')
|
|
-- valuto le corse di 'recupero' dai due carrelli possibili riaccentrando (Y+T) e V
|
|
dCorsaYTr = dCorsaYd + (dYPos -MyMinY)
|
|
dCorsaVr = dCorsaVTd
|
|
bXsw = bVDeltaS and bVxs and dExtraV > dCorsaVr + dCorsaYTr
|
|
end
|
|
-- inizializzo
|
|
dYPosA = dYPos
|
|
dVPosA = dVPos
|
|
dTPosA = dTPos
|
|
end
|
|
|
|
-- **[C1Vs-xsw]** posizione finale di V non raggiungibile, con |dEXtraV > CorsaVr + CorsaYr|
|
|
-- con spostamento richiesto significativo e ExtraV significativo
|
|
while bXsw do
|
|
EmitComment( vCmd, '[C1Vs-xsw]')
|
|
-- **1:** posiziono (Y+T) e V come calcolato sopra
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **2:** allontano Y e (V+T) quanto possibile
|
|
dYPosA = dYPosA + dCorsaYd
|
|
dVPosA = dVPosA - dCorsaVTd
|
|
dTPosA = dTPosA - dCorsaVTd
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- chiudo il carrello Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **3:** accentro (Y+T) e V
|
|
dYPosA = dYPosA - dCorsaYTr -- = MyMinY
|
|
dTPosA = dTPosA - dCorsaYTr
|
|
dVPosA = MaxV
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- valuto i Delta ottenuti
|
|
dYDeltaA = dYPosA - dTPosA
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- aggiorno la verifica di spostamento significativo
|
|
bVDeltaS = ( dVDeltaF > dVDeltaA + dVDeltaTol or dVDeltaF < dVDeltaA - DELTA_SIC)
|
|
-- **aggiorno ExtraV**
|
|
dExtraV = dExtraV - dCorsaYTr - dCorsaVr
|
|
-- **aggiorno la valutazione di ulteriori corse disponibili**
|
|
-- a partire da Y e V c.s. per allontanare q.p. Y e (V+T)
|
|
dCorsaYfc = MaxY - dYPosA
|
|
dCorsaVfc = dVPosA - MinV
|
|
dDistBkN = EMC.LB - dYDeltaA - MinJoin - EMC.TCING -- DistBack 'netta'
|
|
dCorsaYd, dCorsaVTd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistBkN, 'Bk')
|
|
-- aggiorno i prossimi recuperi disponibili
|
|
dCorsaYTr = dCorsaYd
|
|
dCorsaVr = dCorsaVTd
|
|
-- aggiorno verifica per ripetizione del ciclo
|
|
bXsw = bVDeltaS and bVxs and dExtraV > dCorsaVr + dCorsaYTr
|
|
end --[C1Vs-xw]
|
|
|
|
-- **[C1Vs]** |spostamento| richiesto (ev' residuo) |di V| |'significativo'|
|
|
if bVDeltaS then
|
|
-- **[C1Vs-x]** posizione di |V non raggiungibile|
|
|
if dExtraV > 0 then
|
|
-- (dExtraV <= (CorsaYr+CorsaVr) da ciclo precedente)
|
|
-- **[C1Vs-xs]** posizione di V non raggiungibile, |con ExtraV 'significativo'|
|
|
if bVxs then
|
|
local dCorsaYTrA = dYPosA - MyMinY
|
|
-- **[C1Vs-xs1]** se posso recuperare dExtraV solo accentrando V e (Y+T)
|
|
if dCorsaYTrA >= dExtraV then
|
|
EmitComment( vCmd, '[C1Vs-xs1]')
|
|
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **1:** accentro (Y+T) q.b. e V
|
|
dYPosA = dYPosA - dExtraV
|
|
dTPosA = dTPosA - dExtraV
|
|
dVPosA = MaxV -- (pos. finale)
|
|
dVDeltaA = dVPosA - dTPosA
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- **[C1Vs-xs2]** dCorsaYTrA < dExtraV
|
|
else
|
|
EmitComment( vCmd, '[C1Vs-xs2]')
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **1: accentro** ev' (Y+T) e V alle posizioni impostate
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- chiudo (ev') il carrello V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **2:allontano** (V+T) q.b./q.p. e Y per preparare il recupero di ExtraV
|
|
dYPosA, dVPosA, dTPosA = PosXs2Enl( dYPosA, dVPosA, dTPosA, dExtraV, dCorsaYTrA, dCorsaYd, dCorsaVTd, 'V')
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA })
|
|
dYDeltaA = dYPosA - dTPosA
|
|
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **3: accentro**(Y+T) e V (pos. finale)
|
|
dYPosA = MyMinY
|
|
dTPosA = dYPosA - dYDeltaA
|
|
dVPosA = MaxV
|
|
dVDeltaA = dVPosA - dTPosA
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA })
|
|
end -- [C1Vs-xs1/xs2]
|
|
|
|
-- **[C1Vs-xn]** |ExtraV ancora > 0 , ma non 'significativo'| (<= DeltaTol/2)
|
|
else
|
|
EmitComment( vCmd, '[C1Vs-xn]')
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- 1: porto ev' (Y+T) alla posizione impostata sopra e accentro V
|
|
dExtraV = 0
|
|
dVPosA = MaxV -- pos. finale
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
-- chiudo V (?)
|
|
table.insert( vCmd, { 12, 1})
|
|
end -- [C1Vs-xs/xn]
|
|
|
|
-- **[C1Vs-r]** posizione di V 'raggiungibile' (ExtraV <= 0)
|
|
else
|
|
EmitComment( vCmd, '[C1Vs-r]')
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- posizione ev' (Y+T) come impostato sopra e V in posizione finale
|
|
dVPosA = dTPosA + dNewVDelta
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
end -- [C1V-s]
|
|
|
|
-- **[C1Vns]** |spostamento| richIesto per V |non 'significativo'|
|
|
else
|
|
EmitComment( vCmd, '[C1Vns]')
|
|
end
|
|
|
|
-- calcolo i nuovi parametri di aggancio
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, CalcCharStatus( 'V', dVDeltaA)})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- sposto il carrello Y in parcheggio
|
|
table.insert( vCmd, { 1, 'Y', ParkY})
|
|
-- imposto i nuovi parametri di aggancio
|
|
table.insert( vCmd, { 21, 0, dVDeltaA})
|
|
|
|
EgtOutLog( ' VDeltaA =' .. EgtNumToString( dVDeltaA), 1)
|
|
-- reset contatore
|
|
EMC.CNT = nil
|
|
|
|
SpecOutputCNT()
|
|
return vCmd
|
|
end --SpecAdjustCarrC1
|
|
|
|
---------------------------------------------------------------------
|
|
-- *** [C2] da entrambi i carrelli a V : Y+V -> V ***
|
|
---------------------------------------------------------------------
|
|
function SpecAdjustCarrC2( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta)
|
|
EgtOutLog( ' *[C2] = Y+V -> V', 1)
|
|
-- elenco comandi
|
|
local vCmd = {}
|
|
-- Commento
|
|
table.insert( vCmd, { 0, 'Y+V -> V'})
|
|
-- se primo scambio
|
|
local MyMinY = EgtIf( EMC.CNT == 1, MinY + AGG_LOAD, MinY)
|
|
local dCorsaY = MaxY - MyMinY
|
|
local dCorsaV = MaxV - MinV
|
|
-- recupero le posizioni correnti dei carrelli
|
|
local dYPosA = dTPosI + dYDeltaI
|
|
local dVPosA = dTPosI + dVDeltaI
|
|
local dTPosA = dTPosI
|
|
local dYDeltaA = dYDeltaI
|
|
local dVDeltaA = dVDeltaI
|
|
local dNewVDelta -- = dVDeltaF
|
|
-- tolleranze
|
|
local dVDeltaTol = GetDeltaTol( dVDeltaF, EMC.TCING, EMC.HCING, EMC.HOVM, 'V', bFixedDelta)
|
|
-- definisco spostamento 'significativo' in base alle tolleranze
|
|
local bVDeltaS = ( dVDeltaF > dVDeltaA + dVDeltaTol or dVDeltaF < dVDeltaA - DELTA_SIC)
|
|
-- reimposto ev' il delta finale
|
|
if bVDeltaS then
|
|
dNewVDelta = dVDeltaF - dVDeltaTol/2
|
|
else
|
|
dNewVDelta = dVDeltaA
|
|
end
|
|
-- definisco 'ExtraV'
|
|
local dNewV = dTPosI + dNewVDelta
|
|
local dExtraV = dNewV - MaxV -- > 0 se nuova pos. di V 'non raggiungibile' (= oltre MaxV)
|
|
local bVxs = dExtraV > dVDeltaTol/2 -- 'ExtraV significativo'
|
|
|
|
EgtOutLog( ' YDeltaI=' .. EgtNumToString( dYDeltaI) .. ' VDeltaI=' .. EgtNumToString( dVDeltaI) ..
|
|
' VDeltaF=' .. EgtNumToString( dVDeltaF) ,1)
|
|
EgtOutLog( ' NewVDelta=' .. EgtNumToString( dNewVDelta), 1)
|
|
|
|
-- risalita testa a Zmax (da effettuare comunque, per il parcheggio di Y)
|
|
local bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
|
|
-- calcoli preliminari in caso di spostamento richiesto per YV significativo
|
|
local dCorsaYfc, dCorsaVfc, dDistBkN, dCorsaYd, dCorsaVd, dCorsaVTd, dCorsaYTr, dCorsaVr, bXsw
|
|
if bVDeltaS then
|
|
if bVxs then
|
|
-- calcolo le corse disponibili dei carrelli dalle posizioni attuali (=iniziali)
|
|
-- per allontanare Y e (V+T) q.p.
|
|
dCorsaYfc = MaxY - dYPosA
|
|
dCorsaVfc = dVPosA - MinV
|
|
dDistBkN = EMC.LB - dYDeltaA - MinJoin - EMC.TCING -- DistBack 'netta'
|
|
dCorsaYd, dCorsaVTd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistBkN, 'Bk')
|
|
-- valuto le corse di 'recupero' dai due carrelli possibili riaccentrando (Y+T) e V
|
|
dCorsaYTr = dCorsaYd + (dYPosA - MyMinY)
|
|
dCorsaVr = dCorsaVTd
|
|
bXsw = bVDeltaS and bVxs and dExtraV > dCorsaYTr + dCorsaVr
|
|
end
|
|
end
|
|
|
|
-- **[C2Vs-xsw]** posizione finale di V non raggiungibile, |con dEXtraV > CorsaYTr + CorsaVr|
|
|
while bXsw do
|
|
EmitComment( vCmd, '[C2Vs-xsw]')
|
|
|
|
-- chiudo eventualmente il carrello V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- |1:| allontano Y e (V+T) quanto possibile
|
|
dYPosA = dYPosA + dCorsaYd
|
|
dVPosA = dVPosA - dCorsaVTd
|
|
dTPosA = dTPosA - dCorsaVTd
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- chiudo il carrello Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- |3:| accentro (Y+T) e V
|
|
dYPosA = dYPosA - dCorsaYTr
|
|
dTPosA = dTPosA - dCorsaYTr
|
|
dVPosA = MaxV
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
|
|
-- valuto i Delta ottenuti
|
|
dYDeltaA = dYPosA - dTPosA
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- aggiorno la verifica di spostamento significativo
|
|
bVDeltaS = ( dVDeltaF > dVDeltaA + dVDeltaTol or dVDeltaF < dVDeltaA - DELTA_SIC)
|
|
-- **aggiorno ExtraV**
|
|
dExtraV = dExtraV - dCorsaYTr - dCorsaVr
|
|
-- **aggiorno la valutazione di ulteriori corse disponibili**
|
|
dCorsaYfc = MaxY - dYPosA
|
|
dCorsaVfc = dVPosA - MinV
|
|
dDistBkN = EMC.LB - dYDeltaA - MinJoin - EMC.TCING -- DistBack 'netta'
|
|
dCorsaYd, dCorsaVTd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistBkN, 'Bk')
|
|
-- aggiorno i prossimi recuperi disponibili
|
|
dCorsaYTr = dCorsaYd + (dYPosA - MyMinY)
|
|
dCorsaVr = dCorsaVTd
|
|
-- aggiorno verifica per ripetizione del ciclo
|
|
bXsw = bVDeltaS and bVxs and dExtraV > dCorsaYTr + dCorsaVr
|
|
end -- [C2V-xsw]
|
|
|
|
-- **[C2Vs]** se lo |spostamento richiesto per V è significativo|
|
|
if bVDeltaS then
|
|
-- **[C2Vs-x]** posizione di |V non raggiungibile| (oltre MaxV)
|
|
if dExtraV > 0 then
|
|
-- **[C2Vs-xs]** |ExtraV 'significativo'|
|
|
if bVxs then
|
|
local dCorsaYTrA = dYPosA - MyMinY
|
|
-- **[C2Vs-xs1]** se posso recuperare ExtraV semplicem' accentrando V e (Y+T)
|
|
if dCorsaYTrA >= dExtraV then
|
|
EmitComment( vCmd, '[C2Vs-xs1]')
|
|
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **1: accentro** (Y+T) e V
|
|
dYPosA = dYPosA - dExtraV
|
|
dTPosA = dTPosA - dExtraV
|
|
dVPosA = MaxV -- (pos. finale)
|
|
dVDeltaA = dVPosA - dTPosA
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
|
|
else -- **[C2Vs-xs2]**
|
|
EmitComment( vCmd, '[C2Vs-xs2]')
|
|
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, 1})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- **1: allontano** (V+T) e Y
|
|
dYPosA, dVPosA, dTPosA = PosXs2Enl( dYPosA, dVPosA, dTPosA, dExtraV, dCorsaYTrA, dCorsaYd, dCorsaVTd, 'V')
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA })
|
|
dYDeltaA = dYPosA - dTPosA
|
|
|
|
-- chiudo il carrello Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **2: accentro** (Y+T) e V
|
|
dYPosA = MyMinY
|
|
dTPosA = dYPosA - dYDeltaA
|
|
dVPosA = MaxV -- (pos. finale)
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
end -- [C2Vs-xs1/-xs2]
|
|
|
|
-- **[C2Vs-xn]** |dExtraV ancora > 0 ma non 'significativo')|
|
|
else --if dExtraV < DeltaToll/2
|
|
EmitComment( vCmd, '[B1Vs-xn]')
|
|
-- ev' chiudo Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **1:** accentro V
|
|
dExtraV = 0
|
|
dVPosA = MaxV -- (pos. finale)
|
|
table.insert( vCmd, { 1, 'V', dVPosA})
|
|
end -- [C2s-xs/xn]
|
|
|
|
-- **[C2Vs-r]** |dExtraV < 0 (pos. V raggiungibile)|
|
|
else
|
|
EmitComment( vCmd, '[C2Vs-r]')
|
|
-- chiudo ev' Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- porto V in posizione finale
|
|
dVPosA = dTPosA + dNewVDelta
|
|
table.insert( vCmd, { 1, 'V', dVPosA})
|
|
end --[C2Vs]
|
|
|
|
-- **[C2Vns]** = |spostamento di V non significativo|
|
|
else
|
|
EmitComment( vCmd, '[C2Vns]')
|
|
end --[C2Vs/n]
|
|
|
|
-- calcolo il nuovo parametro di aggancio
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- chiudo eventualmente il carrello V e apro Y
|
|
table.insert( vCmd, { 12, CalcCharStatus( 'V', dVDeltaA)})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- sposto il carrello Y in parcheggio
|
|
table.insert( vCmd, { 1, 'Y', ParkY})
|
|
-- imposto il nuovo parametro di aggancio
|
|
table.insert( vCmd, { 21, 0, dVDeltaA})
|
|
|
|
EgtOutLog( ' VDeltaA =' .. EgtNumToString( dVDeltaA), 1)
|
|
-- reset contatore
|
|
EMC.CNT = nil
|
|
|
|
SpecOutputCNT()
|
|
return vCmd
|
|
end --SpecAdjustCarrC2
|
|
|
|
---------------------------------------------------------------------
|
|
-- *** [C3] da carrello V a V : V -> V ***
|
|
---------------------------------------------------------------------
|
|
function SpecAdjustCarrC3( dTPosI, dYDeltaI, dVDeltaI, dTPosF, dYDeltaF, dVDeltaF, bFixedDelta, bFixedPos)
|
|
EgtOutLog( ' *[C3] = V -> V', 1)
|
|
-- elenco comandi
|
|
local vCmd = {}
|
|
-- Commento
|
|
table.insert( vCmd, { 0, 'V -> V'})
|
|
-- se primo scambio
|
|
local MyMinY = EgtIf( EMC.CNT == 1, MinY + AGG_LOAD, MinY)
|
|
-- recupero le posizioni correnti dei carrelli
|
|
local dVPosA = dTPosI + dVDeltaI
|
|
local dYPosA = ParkY
|
|
local dTPosA = dTPosI
|
|
local dVDeltaA = dVDeltaI
|
|
local dYDeltaA = dYPosA - dTPosA
|
|
local dNewVDelta
|
|
-- tolleranza
|
|
local dVDeltaTol = GetDeltaTol( dVDeltaF, EMC.TCING, EMC.HCING, EMC.HOVM, 'V', bFixedDelta)
|
|
local bVDeltaS = (( dVDeltaF > dVDeltaA + dVDeltaTol and not bFixedPos) or dVDeltaF < dVDeltaA - DELTA_SIC)
|
|
if bVDeltaS then
|
|
dNewVDelta = dVDeltaF - dVDeltaTol / 2
|
|
else
|
|
dNewVDelta = dVDeltaA
|
|
end
|
|
EgtOutLog( ' VDeltaI=' .. EgtNumToString( dVDeltaI) .. ' TPosI=' .. EgtNumToString( dTPosI), 1)
|
|
EgtOutLog( ' VDeltaF=' .. EgtNumToString( dVDeltaF) .. ' NewVDelta=' .. EgtNumToString( dNewVDelta), 1)
|
|
-- flag per risalita testa a Zmax
|
|
local bZmaxOk = false
|
|
|
|
-- **[C3Vs]** |pos. di V cambia in modo significativo|
|
|
if bVDeltaS then
|
|
EmitComment( vCmd, '[C3Vs]')
|
|
-- eventuale risalita testa a Zmax
|
|
bZmaxOk = EnsureZmax( bZmaxOk, vCmd)
|
|
-- definisco 'ExtraV' con (V+T) e Y accentrati q.b. per la presa con Y
|
|
local dYPos, dVPos, dTPos = PosxExtraYV( dYPosA, dVPosA, dTPosA, MyMinY, MaxV, 'Y')
|
|
local dNewV = dTPos + dNewVDelta
|
|
local dExtraV = dNewV - MaxV
|
|
-- effettuo spostamenti per predisporre all'aggancio di T con Y
|
|
dYPosA = dYPos
|
|
dVPosA = dVPos
|
|
dTPosA = dTPos
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
dYDeltaA = dYPosA - dTPosA
|
|
|
|
-- **[C3Vs-x]** posizione di |V non raggiungibile| (oltre MaxV)
|
|
if dExtraV > 0 then
|
|
-- **[C3Vs-xs]** posizione di V non raggiungibile, |con ExtraV 'significativo'|
|
|
if dExtraV > dVDeltaTol /2 then
|
|
-- calcolo le **corse disponibili dei carrelli a partire da Y e V c.s.** per allontanare q.p. (V+T) e Y
|
|
local dCorsaYfc = MaxY - dYPos
|
|
local dCorsaVfc = dVPos - MinV
|
|
local dDistBkN = EMC.LB - dYDeltaA - MinJoin - EMC.TCING -- DistBack 'netta'
|
|
local dCorsaYd, dCorsaVTd = GetCorseDisp( dCorsaYfc, dCorsaVfc, dDistBkN, 'Bk')
|
|
-- valuto **le corse di 'recupero'** dai due carrelli possibili riaccentrando (Y+T) e V
|
|
local dCorsaYTr = dCorsaYd + (dYPos - MyMinY)
|
|
local dCorsaVr = dCorsaVTd
|
|
|
|
-- **[C3Vs-xsw]** posizione finale dNewV non raggiungibile, con |dEXtraV > CorsaVr + CorsaYr|
|
|
if dExtraV > ( dCorsaYTr + dCorsaVr) and bVDeltaS then
|
|
EmitComment( vCmd, '[C3Vs-xsw]' .. 'CASO NON GESTITO')
|
|
return
|
|
end
|
|
|
|
local dCorsaYTrA = dYPos - MyMinY -- !! att.ne: non dYPosA !!
|
|
-- **[C3Vs-xs1]** se posso recuperare ExtraV semplicem' accentrando (Y+T) e V
|
|
-- (dalle posizione impostate sopra per l'aggancio di Y)
|
|
if dCorsaYTrA >= dExtraV then
|
|
EmitComment( vCmd, '[C3Vs-xs1]')
|
|
-- chiudo Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **1: accentro** (Y+T) e V
|
|
dYPosA = dYPosA - dExtraV
|
|
dTPosA = dTPosA - dExtraV
|
|
dVPosA = MaxV -- (pos. finale)
|
|
table.insert( vCmd, { 3, 'Y', dYPosA , 'T', dTPosA, 'V', dVPosA})
|
|
dVDeltaA = dVPosA - dTPosA
|
|
|
|
else -- **[C3Vs-xs2]**
|
|
-- ci sarebbe un doppio movimento di V ? => caso impossibile ?
|
|
EmitComment( vCmd, '[C3Vs-xs2]')
|
|
-- **1:** posiziono (ulteriormente!) Y e (V+T)
|
|
dYPosA, dVPosA, dTPosA = PosXs2Enl( dYPosA, dVPosA, dTPosA, dExtraV, dCorsaYTrA, dCorsaYd, dCorsaVTd, 'V')
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA })
|
|
dYDeltaA = dYPosA - dTPosA
|
|
-- chiudo Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **2: accentro** (Y+T) e V
|
|
dYPosA = MyMinY
|
|
dVPosA = MaxV -- (pos. finale)
|
|
dTPosA = dYPosA - dYDeltaA
|
|
table.insert( vCmd, { 3, 'Y', dYPosA, 'T', dTPosA, 'V', dVPosA})
|
|
end -- [C3Vs-xs1/-xs2]
|
|
|
|
-- **[C3Vs-xn]** |dExtraV > 0 ma 'non significativo')|
|
|
else --if dExtraV < DeltaToll/2
|
|
EmitComment( vCmd, '[C3Vs-xn]')
|
|
-- chiudo Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **1:** accentro V
|
|
dExtraV = 0
|
|
dVPosA = MaxV -- (pos. finale)
|
|
table.insert( vCmd, { 1, 'V', dVPosA})
|
|
end --[C3Vs-xs/xn]
|
|
|
|
-- **[C3Vs-r]** |posizione di Y raggiungibile| (ExtraY >=0)
|
|
-- (si esclude la possibilità di extra corsa oltre maxY)
|
|
else
|
|
EmitComment( vCmd, '[C3Vs-r]')
|
|
-- chiudo Y e apro V
|
|
table.insert( vCmd, { 11, 1})
|
|
table.insert( vCmd, { 12, 0})
|
|
-- **2:** posiziono V alla posizione richiesta
|
|
dVPosA = dNewV
|
|
table.insert( vCmd, { 1, 'V', dVPosA})
|
|
end -- [C3Vs]
|
|
|
|
-- calcolo il nuovo parametro di aggancio
|
|
dVDeltaA = dVPosA - dTPosA
|
|
-- chiudo ev' V e apro Y
|
|
table.insert( vCmd, { 12, CalcCharStatus( 'V', dVDeltaA)})
|
|
table.insert( vCmd, { 11, 0})
|
|
-- sposto il carrello Y in parcheggio
|
|
table.insert( vCmd, { 1, 'Y', ParkY})
|
|
-- imposto il nuovo parametro di aggancio
|
|
table.insert( vCmd, { 21, 0, dVDeltaA})
|
|
|
|
-- reset contatore
|
|
EMC.CNT = nil
|
|
|
|
else -- **[C3Vns]** |spostamento| finale richiesto (ev' residuo) di |V non 'significativo'|
|
|
EmitComment( vCmd, '[C3Vns]')
|
|
end --[C3Vs/ns]
|
|
|
|
EgtOutLog( ' VDeltaA =' .. EgtNumToString( dVDeltaA), 1)
|
|
|
|
SpecOutputCNT()
|
|
return vCmd
|
|
end --SpecAdjustCarrC3
|