calc.c

来自「CNC 的开放码,EMC2 V2.2.8版」· C语言 代码 · 共 752 行 · 第 1/2 页

}
/* Element : Monostable (2x2 Blocks) */
void CalcTypeMonostable(int x,int y,StrRung * UpdateRung)
{
    StrMonostable * Monostable;
    Monostable = &MonostableArray[UpdateRung->Element[x][y].VarNum];
    // directly connected to the "left"? if yes, ON !
    if (x==0)
    {
        Monostable->Input = 1;
    }
    else
    {
        Monostable->Input = StateOnLeft(x-1,y,UpdateRung);
    }
    /* detecting impulse on input, the monostable is not retriggerable */
    if (Monostable->Input && !Monostable->InputBak && (Monostable->Value==0) )
    {
        Monostable->OutputRunning = 1;
        Monostable->Value = Monostable->Preset;
    }
    if (Monostable->Value>0)
        Monostable->Value = Monostable->Value - InfosGene->MsSinceLastScan;
    else
        Monostable->OutputRunning = 0;
    Monostable->InputBak = Monostable->Input;
    UpdateRung->Element[x][y].DynamicOutput = Monostable->OutputRunning;
}
/* Element : Counter (2x4 Blocks) */
void CalcTypeCounter(int x,int y,StrRung * UpdateRung)
{
	StrCounter * Counter = &CounterArray[ UpdateRung->Element[x][y].VarNum ];
	// directly connected to the "left"? if yes, ON !
	if ( x==0 )
	{
		Counter->InputReset = 1;
		Counter->InputPreset = 1;
		Counter->InputCountUp = 1;
		Counter->InputCountDown = 1;
	}
	else
	{
		Counter->InputReset = StateOnLeft(x-1,y,UpdateRung);
		Counter->InputPreset = StateOnLeft(x-1,y+1,UpdateRung);
		Counter->InputCountUp = StateOnLeft(x-1,y+2,UpdateRung);
		Counter->InputCountDown = StateOnLeft(x-1,y+3,UpdateRung);
	}
	if ( Counter->InputCountUp && Counter->InputCountUpBak==0 )
	{
		Counter->ValueBak = Counter->Value;
		Counter->Value++;
		if ( Counter->Value>9999 )
			Counter->Value = 0;
	}
	if ( Counter->InputCountDown && Counter->InputCountDownBak==0 )
	{
		Counter->ValueBak = Counter->Value;
		Counter->Value--;
		if ( Counter->Value<0 )
			Counter->Value = 9999;
	}
	if ( Counter->InputPreset )
	{
		Counter->ValueBak = Counter->Value;
		Counter->Value = Counter->Preset;
	}
	if ( Counter->InputReset )
	{
		Counter->ValueBak = Counter->Value;
		Counter->Value = 0;
	}
	Counter->InputCountUpBak = Counter->InputCountUp;
	Counter->InputCountDownBak = Counter->InputCountDown;
	Counter->OutputDone = ( Counter->Value==Counter->Preset )?1:0;
	Counter->OutputEmpty = ( Counter->Value==9999 && Counter->ValueBak==0 )?1:0;
	Counter->OutputFull = ( Counter->Value==0 && Counter->ValueBak==9999 )?1:0;

	UpdateRung->Element[x][y].DynamicOutput = Counter->OutputEmpty;
	UpdateRung->Element[x][y + 1].DynamicOutput = Counter->OutputDone;
	UpdateRung->Element[x][y + 2].DynamicOutput = Counter->OutputFull;
}

/* Element : Compar (3 Horizontal Blocks) */
char CalcTypeCompar(int x,int y,StrRung * UpdateRung)
{
    char State;
    char StateElement;

    StateElement = EvalCompare(ArithmExpr[UpdateRung->Element[x][y].VarNum].Expr);
    UpdateRung->Element[x][y].DynamicState = StateElement;
    if (x==2)
    {
        State = StateElement;
    }
    else
    {
        UpdateRung->Element[x-2][y].DynamicInput = StateOnLeft(x-2,y,UpdateRung);
        State = StateElement && UpdateRung->Element[x-2][y].DynamicInput;
    }
    UpdateRung->Element[x][y].DynamicOutput = State;
    return State;
}

/* Element : Operate (3 Horizontal Blocks) */
char CalcTypeOutputOperate(int x,int y,StrRung * UpdateRung)
{
    char State;
    State = StateOnLeft(x-2,y,UpdateRung);
    if (State)
        MakeCalc(ArithmExpr[UpdateRung->Element[x][y].VarNum].Expr,FALSE /* verify mode */);
    UpdateRung->Element[x][y].DynamicInput = State;
    UpdateRung->Element[x][y].DynamicState = State;
    return State;
}


int RefreshRung(StrRung * Rung, int * JumpTo, int * CallTo)
{
    int x,y;
    int JumpToRung = -1;
    int SectionToCall = -1;

    for (x=0;x<RUNG_WIDTH;x++)
    {
        for(y=0;y<RUNG_HEIGHT;y++)
        {
            switch(Rung->Element[x][y].Type)
            {
                /* MLD,16/5/2001,V0.2.8 , fixed for drawing */
                case ELE_FREE:
                case ELE_UNUSABLE:
                    if (StateOnLeft(x,y,Rung))
                        Rung->Element[x][y].DynamicInput = 1;
                    else
                        Rung->Element[x][y].DynamicInput = 0;
                    break;
                /* End fix */
                case ELE_INPUT:
                    CalcTypeInput(x,y,Rung,FALSE,FALSE);
                    break;
                case ELE_INPUT_NOT:
                    CalcTypeInput(x,y,Rung,TRUE,FALSE);
                    break;
                case ELE_RISING_INPUT:
                    CalcTypeInput(x,y,Rung,FALSE,TRUE);
                    break;
                case ELE_FALLING_INPUT:
                    CalcTypeInput(x,y,Rung,TRUE,TRUE);
                    break;
                case ELE_CONNECTION:
                    CalcTypeConnection(x,y,Rung);
                    break;
                case ELE_TIMER:
                    CalcTypeTimer(x,y,Rung);
                    break;
                case ELE_MONOSTABLE:
                    CalcTypeMonostable(x,y,Rung);
                    break;
                case ELE_COUNTER:
                    CalcTypeCounter(x,y,Rung);
                    break;
                case ELE_COMPAR:
                    CalcTypeCompar(x,y,Rung);
                    break;
                case ELE_OUTPUT:
                    CalcTypeOutput(x,y,Rung,FALSE);
                    break;
                case ELE_OUTPUT_NOT:
                    CalcTypeOutput(x,y,Rung,TRUE);
                    break;
                case ELE_OUTPUT_SET:
                    CalcTypeOutputSetReset(x,y,Rung,FALSE);
                    break;
                case ELE_OUTPUT_RESET:
                    CalcTypeOutputSetReset(x,y,Rung,TRUE);
                    break;
                case ELE_OUTPUT_JUMP:
                    JumpToRung = CalcTypeOutputJump(x,y,Rung);
                    // TODO : abort the refresh of the rung immediately...
                    break;
                case ELE_OUTPUT_CALL:
                    SectionToCall = CalcTypeOutputCall(x,y,Rung);
                    break;
                case ELE_OUTPUT_OPERATE:
                    CalcTypeOutputOperate(x,y,Rung);
                    break;
            }
        }
    }

    *JumpTo = JumpToRung;
    *CallTo = SectionToCall;
    // TODO : tell if the rung isn't finished : if found Call, immediately abort
    // and return here to finish the end, after the sub-routine has been called...
    return TRUE;
}

// we refresh all the rungs of one section.
// we can (J)ump to another rung in the same section.
// we can (C)all a sub-routine (a section) and at the end of it, returns to the
// next rung.
// All the sections 'main' are refreshed in the order defined.
// TODO : if we have a mad Jump (infinite loop) we must make something !
#define SR_STACK 25
void RefreshAllRungs(unsigned long period)
{
    int NumRung;
    int Goto;
    int SectionForCallSR;
    int Done;
    int ScanMainSection;
    int CurrentSection;
    StrSection * pSection;

    int SubRoutinesStack[ SR_STACK ];
    int RungsStack[ SR_STACK ];
    int SubRoutineDepth = 0;

//TODO: times measures should be moved directly in the module task
#ifdef __RTL__
    long StartTime = gethrtime();
#endif
#if defined( RTAI ) && defined( MODULE )
    long StartTime = rt_get_cpu_time_ns();
#endif

    InfosGene->NsSinceLastScan += period;
    InfosGene->MsSinceLastScan = InfosGene->NsSinceLastScan / 1000000;
    InfosGene->NsSinceLastScan %= 1000000;

    CycleStart();

    for ( ScanMainSection=0; ScanMainSection<NBR_SECTIONS; ScanMainSection++ )
    {

        CurrentSection = ScanMainSection;
        pSection = &SectionArray[ CurrentSection ];

        // current section defined and is a main-section (not a sub-routine)
        // and in Ladder language ?
        if ( pSection->Used && pSection->SubRoutineNumber==-1 && pSection->Language==SECTION_IN_LADDER )
        {
            Done = FALSE;
            NumRung = pSection->FirstRung;
            do
            {
                RefreshRung(&RungArray[NumRung], &Goto, &SectionForCallSR);
                if ( SectionForCallSR!=-1 )
                {
                    if ( SectionArray[ SectionForCallSR ].Used && SectionArray[ SectionForCallSR ].SubRoutineNumber>=0 )
                    {
                        if ( SubRoutineDepth<SR_STACK-1 )
                        {
                            // saving current section context
                            SubRoutineDepth++;
                            SubRoutinesStack[ SubRoutineDepth ] = CurrentSection;
                            if ( NumRung!=pSection->LastRung )
                            {
                                RungsStack[ SubRoutineDepth ] = RungArray[NumRung].NextRung;
                            }
                            else
                            {
                                // at the end of the sub-routine, this section is already finished
                                RungsStack[ SubRoutineDepth ] = -1;
                            }
                            // now starting the sub-routine section...
                            CurrentSection = SectionForCallSR;
                            pSection = &SectionArray[ CurrentSection ];
                            NumRung = pSection->FirstRung;
//printf("-> sub-routine called : SR : Sec=%d, Rung =%d\n", CurrentSection, NumRung );
                        }
                        else
                        {
                            debug_printf("Refresh rungs aborted - too much sub-routine calls in series !?");
                        }
                    }
                    else
                    {
                        debug_printf("Refresh rungs aborted - call to a sub-routine undefined or programmed as main !!!");
                    }
                }
                else if ( Goto!=-1 )
                {
                    if (!RungArray[Goto].Used)
                    {
                        Done = TRUE;
                        debug_printf("Refresh rungs aborted - jump to an undefined rung found in rung No%d...\n",Goto);
                    }
                    NumRung = Goto;
                }
                else
                {
                    if (NumRung == pSection->LastRung)
                    {
                        // if this section is a sub-routine, we make a return...
                        if ( pSection->SubRoutineNumber>=0 )
                        {
                            int ReturnFound = FALSE;
                            do
                            {
                                if ( SubRoutineDepth>0 )
                                {

                                    // previous section already finished ?
                                    if ( RungsStack[ SubRoutineDepth ]!=-1 )
                                    {
                                        CurrentSection = SubRoutinesStack[ SubRoutineDepth ];
                                        pSection = &SectionArray[ CurrentSection ];
                                        NumRung = RungsStack[ SubRoutineDepth ];
                                        ReturnFound = TRUE;
//printf( "-> return from sub-routine to : Sec=%d, Rung =%d\n", CurrentSection, NumRung );
                                    }
                                    else
                                    {
                                        // the previous main section was already finished...
                                        if ( SectionArray[ SubRoutinesStack[ SubRoutineDepth ] ].SubRoutineNumber==0 )
                                        {
                                            Done = TRUE;
                                            ReturnFound = TRUE;
//printf( "-> return from sub-routine with previous main section done...\n" );
                                        }
                                        else
                                        {
//printf( "-> return from sub-routine with sub-routine already done... searching again...\n" );
                                        }

                                    }
                                    SubRoutineDepth--;
                                }
                                else
                                {
                                    debug_printf("Refresh rungs aborted - return of sub-routine without call before !?");
                                }
                            }
                            while( !ReturnFound );
                        }
                        else
                        {
                            // last rung found for a main section
                            Done = TRUE;
                        }
                    }
                    else
                        NumRung = RungArray[NumRung].NextRung;
                }
            }
            while(!Done);
        }

#ifdef SEQUENTIAL_SUPPORT
        // current section defined and is in sequential language
        if ( pSection->Used && pSection->Language==SECTION_IN_SEQUENTIAL )
        {
            RefreshSequentialPage( pSection->SequentialPage );
        }
#endif

    }// for( )

    CycleEnd();
//TODO: times measures should be moved directly in the module task
#ifdef __RTL__
    InfosGene->DurationOfLastScan = gethrtime() - StartTime;
//    rtl_printf("Time elapsed for refresh = %u nsec\n",gethrtime()-StartTime);
#endif
#if defined( RTAI ) && defined ( MODULE )
    InfosGene->DurationOfLastScan = rt_get_cpu_time_ns() - StartTime;
#endif
}

void CopyRungToRung(StrRung * RungSrc,StrRung * RungDest)
{
    memcpy(RungDest,RungSrc,sizeof(StrRung));
}