//DEBUGON:
//*******************************************************************************
//* Project      : Demo Program for S300 Demo Set
//* File         : Demok??.PL3 (?? -> Version)
//* Author       : Jochen Karnatz, Kollmorgen Europe GmbH
//* Created      : May 24, 2007
//* last change  : January 26, 2012
//* Drive System : S300/700
//* Firmware     : Version >=2.25
//* Demok04      : English comments and variable names
//* Demok05      : Correction of reference move, one state added
//* Demok06      : Change OPMODE with NOPMODE
//* Demok07      : Set PSTATE 0 and SOFTIN 0 when switching back from clock mode
//*******************************************************************************
// DPRVAR1 = Time in ms to print out the speed
// DPRVAR2 = speed step to add or subtract
// INPUT1 pos. edge increases speed command
// INPUT2 neg. edge decreases speed command
// INPUT3 = 0 : speed mode
//       = 1 : clock mode
// INPUT7 is used for reference move (virtual input) 
//        Precondition: IN7MODE 16, IN7TRIG 0
// OUTPUT1 set when speed command = 0
//*******************************************************************************

//************ initialisation function *********
PROGRAM PLCINIT
LONG PRINTTIME, COUNT1MS, V_INK, V_RPM, ONE, CONSTANT, TEMPVAR1;
LONG SPEEDSTEP, SPEEDCMD, TEMPVAR2, PSTATE, TIMECOUNT, COUNTER;
BYTE PRINT, FLAG1, FLAG2;
PRINT:=0; ONE:=1; SPEEDCMD:=0;PSTATE:=0;
CONSTANT:=240000; //to change V_INC into V_RPM
COMMAND('VREF 360');
COMMAND('NREF 5');
COMMAND('PGEARI 360');
END_PROGRAM

//************ main loop ***********************
PROGRAM PLCMAIN
IF INPUT3=0 THEN
    IF OPMODE=8 THEN
        NOPMODE:=0; FUNCTION(CALCOPMODE); //switch OPMODE
        COMMAND('ALIAS V_STEP');
        PSTATE:=0;
        SOFTIN:=0;
    END_IF;
    PRINTTIME:=DPRVAR1; SPEEDSTEP:=DPRVAR2;
//----------- Print Speed -------------------
    //V_RPM = V_INK * 240000 / 2^32
    IF PRINT THEN
        V_INK:=V;
        TEMPVAR1:=V_INK>>16;
        MULDIV(TEMPVAR1,CONSTANT,ONE,TEMPVAR1);
        V_RPM:=TEMPVAR1>>16;
        IF COUNT1MS> PRINTTIME THEN
            COUNT1MS:=0;
            PRINTF('Speed: %d\n',V_RPM);
        END_IF;
    END_IF;

    IF INPUT1 THEN
        IF FLAG1=0 THEN
            FLAG1:=1;
            SPEEDCMD:=SPEEDCMD+SPEEDSTEP;
        END_IF;
    ELSE
        FLAG1:=0;
    END_IF;
    IF INPUT2 THEN
        IF FLAG2=0 THEN
            FLAG2:=1;
            SPEEDCMD:=SPEEDCMD-SPEEDSTEP;
        END_IF;
    ELSE
        FLAG2:=0;
    END_IF;
    TEMPVAR2:=SPEEDCMD<<16;
    MULDIV(TEMPVAR2,ONE,CONSTANT,TEMPVAR2);
    JCMD:=TEMPVAR2<<16;

//With speed command = 0 Output 1 is set
    IF JCMD:=0 THEN OUTPUT1:=1; ELSE OUTPUT1:=0; END_IF;
//-------------------------- Clock Function ---------------------------------------
ELSE
    IF OPMODE=0 THEN
        NOPMODE:=8;
        FUNCTION(CALCOPMODE);
        SPEEDCMD:=0; //reset command for speed mode
        COMMAND('ALIAS CLOCK');
        IF EN_P_I THEN
           SOFTIN:=SOFTIN|0x04; //set (virt.) Input7 to start reference move
           PSTATE:=1;
        END_IF;
    END_IF;
    IF PSTATE=1 THEN
        TEMPVAR1:=TRJSTAT&0x10000;
        IF TEMPVAR1 THEN //if reference move started (bit "motion task active")
            PSTATE:=2;
        END_IF;
    END_IF;
    IF PSTATE=2 THEN
        TEMPVAR2:=TRJSTAT&0x90000;
        IF TEMPVAR2=0x80000 THEN //if reference move finished
            SOFTIN:=SOFTIN&0xFFFFFFFB; //set Input 7 back to 0
            O_P:=6; O_C:=8195; O_V:=3600; //prepare motion task
            PSTATE:=3;
        END_IF;
    END_IF;
END_IF;
END_PROGRAM


//************ 1 msec function *****************
PROGRAM PLC1
COUNT1MS:=COUNT1MS+1;
//-------------------------- Clock Function ---------------------------------
IF PSTATE>2 THEN //if clock function started and ref. move finished
    TIMECOUNT:=TIMECOUNT+1; //count 1ms cycles
    IF PSTATE=3 THEN
        TIMECOUNT:=0; COUNTER:=0; PSTATE:=4;
    END_IF;
    IF PSTATE=4 THEN
        IF COUNTER<60  THEN //count 60 seconds
            IF TIMECOUNT=1000 THEN
                COUNTER:=COUNTER+1;
                MOVEP_NR:=0;
                SETPTR(TRJ,G_STARTMOVE); //Start motion task
                TIMECOUNT:=0;
            END_IF;
        ELSE
            PSTATE:=5;
        END_IF;
    END_IF;
    IF PSTATE=5 THEN
        IF TIMECOUNT=500 THEN
            O_P:=0; O_C:=8192;
            SETPTR(TRJ,G_STARTMOVE); //drive back to zero
         PSTATE:=6;
        END_IF;
    END_IF;
    IF PSTATE=6 THEN
        TEMPVAR2:=TRJSTAT&0x90000;
        IF TEMPVAR2=0x10000 THEN
        //if "motion task active" and no "In-Position"
            PSTATE:=7;
        END_IF;
    END_IF;
    IF PSTATE=7 THEN
        TEMPVAR2:=TRJSTAT&0x90000;
        IF TEMPVAR2=0x80000 THEN //if motion task finished
            O_P:=6; O_C:=8195; //prepare motion task 0
            PSTATE:=4; COUNTER:=0;
        END_IF;
    END_IF;
END_IF;
END_PROGRAM
