Code et PseudoCode pour initialisation ELE (jouvence)
Decorticage de AXIS_ELE.c:AXIS_ELE_Initialize (glslogin1:~weber/README.Axis_init)
=========================================================================================================
(Attention dans LIDA.c (118), LIDA_ELE_Position)
// On lit qu'une des deux tetes pour tester.
*Position_Mean = *(Position[1]) - MechanicalOffset[1];
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
Une modif avec Bruno pour retirer la deuxieme tete (qui est defecteuse)
=========================================================================================================
OFFSET: Equivalence de nom:
Ces offsets sont les offsets des 2 tetes (ele) par rapport a l'offset fourni par la config
mesures lors de la detection des 2 top0 (4 valeurs en tout), au final seul les offsets de
la premiere tete sont utilises (on les mesure tous car il peuvent venir dans un ordre
different (idem pour azi)
long ELE_LIDA_Offsets [LIDA_ELE_NUMBEROFHEADS*LIDA_ELE_NUMBEROFHEADS],
long AXIS_LIDA_Offsets[LIDA_ELE_NUMBEROFHEADS*LIDA_ELE_NUMBEROFHEADS]
long MechanicalOffset [LIDA_ELE_NUMBEROFHEADS*LIDA_ELE_NUMBEROFHEADS]
=========================================================================================================
Calcul de la position:
(attention dans LIDA.c (118), LIDA_ELE_Position)
int LIDA_ELE_Position (long MechanicalOffset[LIDA_ELE_NUMBEROFHEADS*LIDA_ELE_NUMBEROFHEADS], long* Position[LIDA_ELE_NUMBEROFHEADS], double* Position_Mean)
// appel MechanicalOffset:= ELE_LIDA_Offsets
// avec: long ELE_LIDA_Offsets[LIDA_ELE_NUMBEROFHEADS*LIDA_ELE_NUMBEROFHEADS];
//
//
for(i=0; LIDA_ELE_NUMBEROFHEADS>i; i++){
*(Position[i])= *(Position[i]) - MechanicalOffset[i];
Positions_Sum= Positions_Sum + *(Position[i]);
*Position_Mean=((double) Positions_Sum)/((double) LIDA_ELE_NUMBEROFHEADS);
}
on n'utilise que AXIS_LIDA_Offsets[0] et AXIS_LIDA_Offsets[1]
=========================================================================================================
axes definition rot = 1
azi = 2
ele = 4
reg = 8
static tsrvCOMMAND REGsrvCommands[] =
{ "axis.init", AxisInit, "AxisInit", "", "s", "s"}, (treglib.h)
AxisInit(tregAxisOperations.c)
AXIS_ELE_Initialize(AXIS_ELE.c)
static tsrvCOMMAND REGsrvCommands[] =
{ "axis.sync.init", AxisSyncInit, "AxisSyncInit", "", "s", "sss"}, (treglib.h)
AxisSyncInit(tregAxisOperations.c)
t_ele_AxisSyncInit(tregAxisOperations.c)
AXIS_ELE_Initialize(AXIS_ELE.c)
static tsrvCOMMAND REGsrvCommands[] =
{ "axis.set.offset", axisSetOffset, "change offset. proc can be azi, ele, rot. Case insensitive.", "", "ss", "s"},
axisSetOffset(tregAxisOperations.c) ELE_INITIALIZATION_Offset=d0;
################################################################################################
Le code qui suit est l'original mais la gestion des erreurs est supprimee pour plus de clarete
log de: $TDATA/services/MAINTENANCE/treg_srv/treg_srv_20210718.log
################################################################################################
int AXIS_ELE_Initialize () // (inter:"axis.init")
{
Ok = SCT_To_LIDA_AZI_Position (AXIS_INITIALIZATION_Offset, &AXIS_INITIALIZATION_GESINC4_Value); // AXIS_INITIALIZATION_Offset == ELE_INITIALIZATION_Offset (inter:"axis.set.offset")
/#/ AXIS_INITIALIZATION_GESINC4_Value = ((AXIS_INITIALIZATION_Offset /(2.0*DPI))* LIDA_AZI_RESOLUTION) // t120_axisinit.prc:axis /offset=te.ele.off /ele /noecho
// global_te.prc:global te.ele.off=71.83276
// en coordonee du compteur LIDA = 3591637 (0..18'000'000)
/* Initialisation de CMASKA a la valeur par defaut */
/*(butees logicielles ST1 PHIL1/PHIL2 inactives) */
ST1_SetPar (ST1_Serial, ST1_CMASKA, ELE_ST1_CMASKA_DEFAULT, ST1_StatA, NULL, &Error); // CMaskA == defaut
/* Initialisation de CMASKS a la valeur par defaut */
/*(butees logicielles ST1 PHIL1/PHIL2 inactives) */
ST1_SetPar (ST1_Serial, ST1_CMASKS, ELE_ST1_CMASKS_DEFAULT, ST1_StatA, NULL, &Error); // CMaskS == defaut
AXIS_ELE_Ask_Power (ST1_Connection, ST1_Serial, ST1_Power, ST1_StatD;
//////////////////////////////////////////////////////////////////////////////////////////////////
1 AXIS_GoToStartPosition (...); // goto start
GESINC4_EnableTOP0(ENCODER_ELE_SUBDEVICEOFFSET); // == 4 (ELE 1) ---- ENABLE TOP0
/#/ void NI6602indexG0Enable(tTIO6602 **board)
GESINC4_EnableTOP0(ENCODER_ELE_SUBDEVICEOFFSET+1); // == 5 (ELE 2)
*CONSIGNE_Velocity = (double) AXIS_INIT_ABSOLUTESPEED_LOW; // == 0.2[d/s] (Displacement LOW at 0.003491 speed)
SCT_Execute_Run_1 (...); // deplacement vitesse
msg_printf(MSG_AXIS, "%s: Wait for any top 0\n",__func__);
1626663791 882274 00:03:11 | TREG_SRV:AXIS:AXIS_ELE_Initialize: Wait for any top 0
PValue1 = AXIS_INITIALIZATION_GESINC4_Value; // PValue1 = 3591637 (71.83276[d])
PValue2 = AXIS_INITIALIZATION_GESINC4_Value; // PValue2 = 3591637 (71.83276[d])
GESINC4_Write2Counters32(ENCODER_ELE_SUBDEVICEOFFSET, PValue1 , PValue2); // Write2Counters32
/#/ void NI6602writeG0Counter(iBus *bus, tTIO6602 **board, unsigned long value)
int topok=0;
int i=0;
GESINC4_FlushALL2Heads(); // 5x read status TOP0 2 tetes (flush)
while(topok!=3) { // condition d'arret: 2 top0 lus
i+=1;
GESINC4_WaitForAny2TOP0AndDisable (24000000, &Offset); // bloquant, timeout=24[s], return device offset (4 ou 5)
/#/ int NI6602_ReadTop0
/#/ int NI6602readG0TCStatus(tTIO6602 **board)
/#/ status = (*board)->G0_Status_1_Register.readG0_Gate_Interrupt_St();
/#/ if (status) {
/#/ (*board)->Interrupt_G0_Ack_Register.writeG0_Gate_Interrupt_Ack(1);
/#/ (*board)->Interrupt_G0_Ack_Register.writeG0_TC_Interrupt_Ack(1);
/#/ }
/#/ int NI6602_DisableTop0 (si detection)
/#/ void NI6602indexG0Disable
// Normallement Disable du TOP0 detecte
msg_printf(MSG_AXIS, "%s: AXIS_ELE Top 0 found for counter[%d] %d\n",__func__, i, Offset);
1626663796 714849 00:03:16 | TREG_SRV:AXIS:AXIS_ELE_Initialize: AXIS_ELE Top 0 found for counter[1] 4
1626663796 714984 00:03:16 | TREG_SRV:AXIS:AXIS_ELE_Initialize: AXIS_ELE Top 0 found for counter[2] 4 // le disable ne marche pas si on detecte 2 fois le 4 !!!
1626663797 482849 00:03:17 | TREG_SRV:AXIS:AXIS_ELE_Initialize: AXIS_ELE Top 0 found for counter[3] 5
if (Offset == ENCODER_ELE_SUBDEVICEOFFSET) topok = topok | 1; // set mask selon offset
if (Offset == ENCODER_ELE_SUBDEVICEOFFSET+1) topok = topok | 2;
1626663791 882331 00:03:11 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: TimeOut 10991.882324 24000000 // 10991882324 11 digits (heure GMT en musec)
1626663796 714724 00:03:16 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: Found TOP0 Counter 4 // donc 10991.882324[s] (max 86400)
1626663796 714867 00:03:16 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: TimeOut 10996.714861 24000000
1626663796 714967 00:03:16 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: Found TOP0 Counter 4
1626663796 715000 00:03:16 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: TimeOut 10996.714994 24000000
1626663797 482815 00:03:17 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: Found TOP0 Counter 5
}
*CONSIGNE_Velocity = AXIS_INIT_ABSOLUTESPEED_STOP; // STOP
SCT_Execute_Run_1 (...); //
//////////////////////////////////////////////////////////////////////////////////////////////////
2 AXIS_GoToStartPosition (...); // retour a la position TplusR
GESINC4_EnableTOP0(ENCODER_ELE_SUBDEVICEOFFSET); // meme operation que le tour d'avant ??? ---- ENABLE TOP0
GESINC4_EnableTOP0(ENCODER_ELE_SUBDEVICEOFFSET+1); //
*CONSIGNE_Velocity = AXIS_INIT_ABSOLUTESPEED_LOW;
SCT_Execute_Run_1 (...);
GESINC4_Read2Counters32 (ENCODER_ELE_SUBDEVICEOFFSET, &PValue1, &PValue2); // Read2Counters32 (Tour 1: Write2Counters32) ref;3591637
1626663806 733080 00:03:26 | TREG_SRV:AXIS:AXIS_ELE_Initialize: Value counter before second TOP 0 3544406 3536737 ------------------------------------------------
/#/ unsigned long readG0Counter(tTIO *board){
int topok=0;
int i=0;
GESINC4_FlushALL2Heads();
while(topok!=3) {
int LIDA_HeadIndex;
i++;
GESINC4_WaitForAny2TOP0AndDisable (24000000, &Offset); // bloquant, timeout=24[s], return device offset (4 ou 5)
// Normallement Disable du TOP0 detecte
GESINC4_Latch2Counter32 (ENCODER_ELE_SUBDEVICEOFFSET); // READ 2 compteur et store dans CounterLatchedValue[4,5]
/#/ int GESINC4_Latch2Counter32 (int subdeviceOffset){
/#/ long Value1, Value2;
/#/
/#/ checkErr(subdeviceOffset 5);
/#/ if (subdeviceOffset 7)
/#/ return 1;
/#/ GESINC4_ReadCounters32(subdeviceOffset, &Value1);
/#/ GESINC4_ReadCounters32(subdeviceOffset+1, &Value2);
/#/ CounterLatchedValue[subdeviceOffset] = Value1;
/#/ CounterLatchedValue[subdeviceOffset+1] = Value2;
/#/ return 0;
/#/ }
if (Offset == ENCODER_ELE_SUBDEVICEOFFSET) topok = topok | 1; // offset == 4
if (Offset == ENCODER_ELE_SUBDEVICEOFFSET+1) topok = topok | 2; // offset == 5
1626663806 733110 00:03:26 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: TimeOut 11006733104 24000000
1626663811 552139 00:03:31 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: Found TOP0 Counter 4
1626663811 574291 00:03:31 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: TimeOut 11011574285 24000000
1626663811 574339 00:03:31 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: Found TOP0 Counter 4
1626663811 574374 00:03:31 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: TimeOut 11011574368 24000000
1626663812 320129 00:03:32 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: Found TOP0 Counter 5
if (ST1_Connection && (i == 1)) {
/* Initialisation de l'echelle de position du servo-ampli */
msg_printf(MSG_AXIS, "%s: ST1 initialization PwrOnI CMASKA\n",__func__);
1626663811 552262 00:03:31 | TREG_SRV:AXIS:AXIS_ELE_Initialize: ST1 initialization PwrOnI CMASKA
ST1_PwrOnI (ST1_Serial, ST1_StatA, NULL, &Error);
ST1_IniAbs (ST1_Serial, (int) 0, ST1_StatA, NULL, &Error);
/* SERVO-AMPLIFICATEUR ENTRAINEMENT */
/* Initialisation du parametre CMASKA */
/* Activation des butees logicielles de position */
ST1_SetPar (ST1_Serial, ST1_CMASKA, ELE_ST1_CMASKA, ST1_StatA, NULL, &Error);
}
LIDA_HeadIndex = Offset-ENCODER_ELE_SUBDEVICEOFFSET; // (4-4 ou 5-4 (selon offset)) LIDA_HeadIndex == 0 ou 1
GESINC4_Read2LatchedCounters32 (ENCODER_ELE_SUBDEVICEOFFSET, &(InitialPosition[0]), &(InitialPosition[1])); // retourne simplement CounterLatchedValue[4,5] latchee plus haut
msg_printf(MSG_AXIS, "%s: InitialPosition %ld %ld\n",__func__,InitialPosition[0], InitialPosition[1]);
1626663811 574269 00:03:31 | TREG_SRV:AXIS:AXIS_ELE_Initialize: InitialPosition 3591637 3583967
1626663811 574358 00:03:31 | TREG_SRV:AXIS:AXIS_ELE_Initialize: InitialPosition 3591812 3584163
1626663812 320154 00:03:32 | TREG_SRV:AXIS:AXIS_ELE_Initialize: InitialPosition 3599291 3591638
AXIS_LIDA_Offsets[LIDA_HeadIndex*LIDA_ELE_NUMBEROFHEADS+0] = InitialPosition[0] - AXIS_INITIALIZATION_GESINC4_Value; // indice == 0*2+0 (0) || 1*2+0 (2)
AXIS_LIDA_Offsets[LIDA_HeadIndex*LIDA_ELE_NUMBEROFHEADS+1] = InitialPosition[1] - AXIS_INITIALIZATION_GESINC4_Value; // indice == 0*2+1 (1) || 1+2+1 (3)
// en fin de boucle on a rempli:
// AXIS_LIDA_Offsets[0] = (compteur[0]-offset(configuration)) a la detection du top0[0]
// AXIS_LIDA_Offsets[1] = (compteur[1]-offset(configuration)) a la detection du top0[0]
// AXIS_LIDA_Offsets[2] = (compteur[0]-offset(configuration)) a la detection du top0[1]
// AXIS_LIDA_Offsets[3] = (compteur[1]-offset(configuration)) a la detection du top0[1]
// Selon juste ci-dessus:
// InitialPosition 3591637 3583967 => 0 -7670
// InitialPosition 3591812 3584163 => 175 -7474 Dur a comprendre pourquoi on a 2 top zero de ta tete 0 qui se suivent ???!!!
// InitialPosition 3599291 3591638 => 7654 1
}
int i;
for (i = 0; LIDA_ELE_NUMBEROFHEADS > i; i++) {
msg_printf(MSG_AXIS, "%s: AXIS_ELE offset[%d] %ld %ld\n", __func__, i,
AXIS_LIDA_Offsets[i*LIDA_ELE_NUMBEROFHEADS+0],
AXIS_LIDA_Offsets[i*LIDA_ELE_NUMBEROFHEADS+1]);
1626663812 320168 00:03:32 | TREG_SRV:AXIS:AXIS_ELE_Initialize: AXIS_ELE offset[0] 175 -7474 // (delta=7649)
1626663812 320183 00:03:32 | TREG_SRV:AXIS:AXIS_ELE_Initialize: AXIS_ELE offset[1] 7654 1 // (delta=7653)
}
CONSIGNE_Velocity = AXIS_INIT_ABSOLUTESPEED_STOP; // STOP
SCT_Execute_Run_1 (...); // STOP
//////////////////////////////////////////////////////////////////////////////////////////////////
/* * Initialisation echelle ST1 * */
3 AXIS_GoToStartPosition (.);
*CONSIGNE_Velocity = AXIS_INIT_ABSOLUTESPEED_LOW/2.;
SCT_Execute_Run_1 (...);
int i=0;
GESINC4_Read2Counters32 (ENCODER_ELE_SUBDEVICEOFFSET, &PValue1, &PValue2);
msg_printf(MSG_AXIS, "%s: Value counter before second TOP 0 %ld %ld\n",__func__, PValue1, PValue2);
3544406 3536737
1626663821 428194 00:03:41 | TREG_SRV:AXIS:AXIS_ELE_Initialize: Value counter before second TOP 0 3544206 3536558 ------------------------------------------------
int topok=0;
GESINC4_FlushALL2Heads();
while(topok!=3) {
int LIDA_HeadIndex;
i++;
GESINC4_WaitForAny2TOP0AndDisable (24000000, &Offset); // bloquant, timeout=24[s], return device offset (4 ou 5)
if (Offset == ENCODER_ELE_SUBDEVICEOFFSET) topok = topok | 1;
if (Offset == ENCODER_ELE_SUBDEVICEOFFSET+1) topok = topok | 2;
1626663821 428224 00:03:41 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: TimeOut 11021428218 24000000
1626663830 954823 00:03:50 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: Found TOP0 Counter 4
1626663830 985008 00:03:50 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: TimeOut 11030985003 24000000
1626663830 985027 00:03:50 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: Found TOP0 Counter 4
1626663831 014349 00:03:51 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: TimeOut 11031014344 24000000
1626663832 489761 00:03:52 | TREG_SRV:AXIS:GESINC4_WaitForAny2TOP0AndDisable: Found TOP0 Counter 5
LIDA_HeadIndex = Offset-ENCODER_ELE_SUBDEVICEOFFSET;
ST1_AskPos (ST1_Serial,(int *) &(AXIS_ST1_Resolver_Offsets[LIDA_HeadIndex]),NULL, &Error); // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
msg_printf(MSG_AXIS, "%s: ST1_AskPos read POS %d\n",__func__, AXIS_ST1_Resolver_Offsets[Offset]); // bug affichage (raison du zero)
// doit etre: AXIS_ST1_Resolver_Offsets[LIDA_HeadIndex]
1626663830 984988 00:03:50 | TREG_SRV:AXIS:AXIS_ELE_Initialize: ST1_AskPos read POS 0
1626663831 014332 00:03:51 | TREG_SRV:AXIS:AXIS_ELE_Initialize: ST1_AskPos read POS 0
1626663832 519651 00:03:52 | TREG_SRV:AXIS:AXIS_ELE_Initialize: ST1_AskPos read POS 0
}
*CONSIGNE_Velocity = AXIS_INIT_ABSOLUTESPEED_STOP; // STOP
SCT_Execute_Run_1 (...); //
//////////////////////////////////////////////////////////////////////////////////////////////////
Fin if (ST1_Connection) {
/* Initialisation de CMASKA */
/*(butees logicielles ST1 PHIL1/PHIL2 actives) */
ST1_SetPar (ST1_Serial, ST1_CMASKA, ELE_ST1_CMASKA, ST1_StatA, NULL, &Error); // definit CMaskA
/* Initialisation de CMASKS */
/*(butees logicielles ST1 PHIL1/PHIL2 actives) */
ST1_SetPar (ST1_Serial, ST1_CMASKS, ELE_ST1_CMASKS, ST1_StatA, NULL, &Error); // definit CMaskS
}
msg_printf(MSG_AXIS, "%s: Set CMASKA CMASKSn",__func__, AXIS_INITIALIZATION_GESINC4_Value);
1626663832 556314 00:03:52 | TREG_SRV:AXIS:AXIS_ELE_Initialize: Set CMASKA CMASKSn
*LIDA_Initialization = 1;
msg_printf(MSG_AXIS, "%s: LIDA Initialization OK\n",__func__);
GESINC4_DisableTOP0(ENCODER_ELE_SUBDEVICEOFFSET); ---- DISABLE TOP0
GESINC4_DisableTOP0(ENCODER_ELE_SUBDEVICEOFFSET+1);
return Error ? Error + AXIS_ERRORNUMBER_BASE : 0;
}
############################################################
Pseudo Code
############################################################
Il faut savoir que le code (des le debut jouvence) avait des comportements non-desire incompris (dixit Charles)
En 2019 ces problemes ont ete compris et resolus mais on amene d'autres comportement, dans le 2 cas le
systeme etait fonctionnel.
jouvence: le zero etait fait selon les T+R donc mauvaise precision
2019: le zero se fait sur les top-zero LIDA (la solution etait de flusher la lecture des top zero)
L'anlyse du code a permis de mieux comprendre le fonctionnement de la carte NI6602 et permet d'envisager une modification
du code pour obtenir une meilleure precision accelerer le temps d'intialisation en passant de 3 a 2 passages (voir 3 a 1) ou pas!
Ce qu'il faut savoir pour comprendre le code:
la communication avec la carte NI662 est a la vitesse du processeur
l'initialisation du top zero devrait etre instantanee dans la carte
la communication avec le ST1 est une ligne serie donc lente
Ces differents elements justifient (ou non) les multiples passages
Concernant la carte et le code (TBC):
- les top zero sont toujours detectes par NI6602readG0TCStatus (valeur de status)
- GESINC4_Write2Counters32 ecrit dans un "registre de preset" et c'est le top zero qui update le compteur.
- le top zero update le compteur uniquement si est enable
- il n'y a pas d'autre moyen de faire un preset du compteur (TBC) donc il faut lire les 2 tops zero
- comme on lit des tops zero decales on mesure l'offset entre les deux (MechanicalOffset == AXIS_LIDA_Offsets == ELE_LIDA_Offsets)
- de toute evidence le MechanicalOffset doit etre 0 et n ou n et 0 selon l'orde de detection
- le disable n'empeche pas la detection du top zero mais empeche le preset du compteur
- GESINC4_Read2Counters32 lit effectivement le compteur
- visiblement la carte enregistre (dans un stack (TBC) les top zero), le flush a resolu ce probleme (GESINC4_FlushALL2Heads), sans
cela on detecte immediatement un top zero sans top zero, typiquement a la position d'arrivee selon T+R (bug initial)
Ainsi:
- le enable des top zero ne devrait se faire qu'une fois et non chaque passage
- si les tetes sont proches, une communication avec le ST1 peut compromettre la detection de la deuxieme tete,
ainsi il faut priviligier une vitesse plus lente si on ne veut fairequ'un passage
Debut
convertion offset(inter:te.ele.off) degre en offset valeur LIDA (SCT_To_LIDA_AZI_Position)(dans AXIS_INITIALIZATION_GESINC4_Value)
CMASKA CMASKS mis au defauts (ST1_SetPar)
---------------------
Passage 1: Preset des compteur a la valeur d'offset donnee dans la configuration (inter:te.ele.off)
goto start position (mode TplusR) (AXIS_GoToStartPosition)
enable des 2 top0 (GESINC4_EnableTOP0)
avance lente mode vitesse (SCT_Execute_Run_1)--------------\
ecriture des valeurs d'offset (configuration) dans les registres (GESINC4_Write2Counters32)--------+- Ces 2 lignes devraient etre inversees !!
lecture (flush) a vide des compteurs (GESINC4_FlushALL2Heads)
@boucle de detection des 2 top zero (bloquant avec timeout):
bloquant jusqu'a detection (GESINC4_WaitForAny2TOP0AndDisable)
si detection: (NI6602_ReadTop0 + NI6602_DisableTop0)
affichage
lorsque les 2 top0 sont detectes:
STOP (SCT_Execute_Run_1)
---------------------
Passage 2: Met a jour le tableau AXIS_LIDA_Offsets pour le calcul de la position depuis les compteur LIDA
// AXIS_LIDA_Offsets[0] = (compteur[0]-offset(configuration)) a la detection du top0[0]
// AXIS_LIDA_Offsets[1] = (compteur[1]-offset(configuration)) a la detection du top0[0]
// AXIS_LIDA_Offsets[2] = (compteur[0]-offset(configuration)) a la detection du top0[1]
// AXIS_LIDA_Offsets[3] = (compteur[1]-offset(configuration)) a la detection du top0[1]
// Rem: le sens de detection peut etre 0-1, 2-3 ou 2-3, 0-1
// par la suite on n'utilise que AXIS_LIDA_Offsets[0] et AXIS_LIDA_Offsets[1]
goto start position (mode TplusR) (AXIS_GoToStartPosition)
enable des 2 top0 (GESINC4_EnableTOP0)
avance lente mode vitesse (SCT_Execute_Run_1)-------------\
lecture des compteurs et affichage (GESINC4_Read2Counters32)--------+- Ces 2 lignes devraient etre inversees !!
lecture (flush) a vide des compteurs (GESINC4_FlushALL2Heads)
@boucle de detection des 2 top zero (bloquant avec timeout):
bloquant jusqu'a detection (GESINC4_WaitForAny2TOP0AndDisable)
si detection: (NI6602_ReadTop0 + NI6602_DisableTop0)
memorise a chaque detection les 2 compteurs dans CounterLatchedValue[4]&[5] (GESINC4_Latch2Counter32)
a la premiere detection:
ST1_PwrOnI()
ST1_IniAbs()
ST1_SetPar()
LIDA_HeadIndex = Offset(4||5)-ENCODER_ELE_SUBDEVICEOFFSET(4); (pour le calcul de l'index dans tableau AXIS_LIDA_Offsets (0 ou 1)) // (4-4 ou 5-4 (selon offset)) LIDA_HeadIndex == 0 ou 1
mise a jour jour de InitialPosition[0] et [1] depuis CounterLatchedValue[4] et [5] (GESINC4_Read2LatchedCounters32)
mise a jour de AXIS_LIDA_Offsets[i]||[i+1] a InitialPosition[i]&[i+1]-AXIS_INITIALIZATION_GESINC4_Value (i=No de tete 0 ou 1)
lorsque les 2 top0 sont detectes:
print de AXIS_LIDA_Offsets[i]&[i+1] (4 values)
STOP (SCT_Execute_Run_1)
---------------------
Passage 3: Met a jour AXIS_ST1_Resolver_Offsets
rem: pas de enable des top0
goto start position (mode TplusR) (AXIS_GoToStartPosition)
avance (moitie de vitesse) lente, mode vitesse (SCT_Execute_Run_1)
lecture des compteurs et affichage (GESINC4_Read2Counters32)
lecture (flush) a vide des compteurs (GESINC4_FlushALL2Heads)
@boucle de detection des 2 top zero (bloquant avec timeout): (rem: devrait etre 2xplus long)
bloquant jusqu'a detection (GESINC4_WaitForAny2TOP0AndDisable)
si detection: (NI6602_ReadTop0 + NI6602_DisableTop0)
LIDA_HeadIndex = Offset-ENCODER_ELE_SUBDEVICEOFFSET;
enregistre l'offset du resolveur (pour les EAmove) (ST1_AskPos:&(AXIS_ST1_Resolver_Offsets[LIDA_HeadIndex]))
affichage position resolveur (mais bug) (ST1_AskPos)
lorsque les 2 top0 sont detectes:
print de AXIS_LIDA_Offsets[i]&[i+1]
STOP (SCT_Execute_Run_1)
---------------------
Fin
CMASKA CMASKS mis a leur valeur de travail (ST1_SetPar)
Disabled des 2 top zeros (GESINC4_DisableTOP0)
###########################################################################################################
###########################################################################################################
PROPOSITION en 1 seul passage:
Debut
convertion offset(inter:te.ele.off) degre en offset valeur LIDA (SCT_To_LIDA_AZI_Position)(dans AXIS_INITIALIZATION_GESINC4_Value)
CMASKA CMASKS mis au defauts (ST1_SetPar)
---------------------
goto start position (mode TplusR) (AXIS_GoToStartPosition)
enable des 2 top0 (GESINC4_EnableTOP0)
ecriture des valeurs d'offset (configuration) dans les registres (GESINC4_Write2Counters32)
lecture des compteurs et affichage (GESINC4_Read2Counters32) (pour info et voir que le write2counter ne change pas les compteurs)
lecture (flush) a vide des compteurs (GESINC4_FlushALL2Heads)
avance (moitie de vitesse) lente, mode vitesse (SCT_Execute_Run_1)
@boucle de detection des 2 top zero (bloquant avec timeout):
bloquant jusqu'a detection (GESINC4_WaitForAny2TOP0AndDisable)
si detection: (NI6602_ReadTop0 + NI6602_DisableTop0)
memorise a chaque detection les 2 compteurs dans CounterLatchedValue[4]&[5] (GESINC4_Latch2Counter32)
a la premiere detection:
ST1_PwrOnI()
ST1_IniAbs()
ST1_SetPar()
// mechanical offset
LIDA_HeadIndex = Offset(4||5)-ENCODER_ELE_SUBDEVICEOFFSET(4); (pour le calcul de l'index dans tableau AXIS_LIDA_Offsets (0 ou 1)) // (4-4 ou 5-4 (selon offset)) LIDA_HeadIndex == 0 ou 1
mise a jour jour de InitialPosition[0] et [1] depuis CounterLatchedValue[4] et [5] (GESINC4_Read2LatchedCounters32)
mise a jour de AXIS_LIDA_Offsets[i]||[i+1] a InitialPosition[i]&[i+1]-AXIS_INITIALIZATION_GESINC4_Value (i=No de tete 0 ou 1)
// resolveur
LIDA_HeadIndex = Offset-ENCODER_ELE_SUBDEVICEOFFSET;
enregistre l'offset du resolveur (pour les EAmove) (ST1_AskPos:&(AXIS_ST1_Resolver_Offsets[LIDA_HeadIndex]))
affichage position resolveur (mais bug) (ST1_AskPos)
lorsque les 2 top0 sont detectes:
print de AXIS_LIDA_Offsets[i]&[i+1] (4 values)
STOP (SCT_Execute_Run_1)
---------------------
Fin
CMASKA CMASKS mis a leur valeur de travail (ST1_SetPar)
Disabled des 2 top zeros (GESINC4_DisableTOP0)