cnc-club.ru

component revolverka;

pin out bit delaystart = 0; //0,15сек
pin in bit delaydone = 0;
pin out bit delaystart2 = 0; //1сек
pin in bit delaydone2 = 0;
pin out bit delaystart3 = 0; //2сек
pin in bit delaydone3 = 0;

pin in bit toolchange ; // смена инструмент tool-change команда на смену инстркмента 1 или 0 >> от чпу
pin in s32 toolnumber ; // установленный инструмент tool-number >> от чпу
pin out bit toolchanged ; // окончание смены инструмента tool-changed << в чпу
pin in s32 old; // установленный инструмент tool-oldnumber >> от чпу
pin out s32 position = 0; // инструмент по датчикам
pin out s32 tool = 0;
pin out float e = 0;
pin out float a = 0;



pin out bit moto=0; //out02 крутим назад, если 1 крутим вперед

pin in bit opto1 = 0; // датчик1 in10
pin in bit opto2 = 0; // датчик2 in11
pin in bit opto3 = 0; // датчик3 in12
pin in bit metka = 0; // датчик4 in13


option singleton yes;
function _;
author "WV";
license "GPL";
;;

// rtapi_print_msg(RTAPI_MSG_ERR, "Error - opto inputs do not match truth table");

FUNCTION(_)
{



if(!opto1 && !opto2 && opto3 && metka==0)
position = 1;
else if(!opto1 && opto2 && !opto3 && metka==0)
position = 2;
else if(!opto1 && opto2 && opto3 && metka==0)
position = 3;
else if(opto1 && !opto2 && !opto3 && metka==0)
position = 4;
else if(opto1 && !opto2 && opto3 && metka==0)
position = 5;
else if(opto1 && opto2 && !opto3 && metka==0)
position = 6;


if (toolchange)
{tool=toolnumber;}

switch ( tool )
{

case 1:

if(position !=1 )
{
moto = 1; toolchanged = 0; //вперед
}


if (position==1 && metka==1 && e==0) //реверс
{
delaystart = 1; //0,15 sec антидребезг
if(delaydone==1)
{
delaystart = 0;
if (position==1 && metka==1)
{ moto = 0; e=1; }

}
}

if (e==1)
{
if (position==1 && metka==0 ) //проверка после реверса
{
if (a==0)
{
delaystart2 = 1; //1sec
a=1;
}
if(delaydone2)
{
delaystart2 = 0;
}
if (position==1 && metka==0 && delaydone2==0 )
{
delaystart3 = 1; //2sec

if(delaydone3)
{
delaystart3 = 0;
toolchanged = 1; //инструмент готов на чпу
e=0;
a=0;
}
}
}
}


break;

case 2:

if(position !=2 )
{
moto = 1; toolchanged = 0; //вперед
}


if (position==2 && metka==1 && e==0) //реверс
{
delaystart = 1; //0,15 sec антидребезг
if(delaydone==1)
{
delaystart = 0;
if (position==2 && metka==1)
{ moto = 0; e=1; }

}
}

if (e==1)
{
if (position==2 && metka==0 ) //проверка после реверса
{
if (a==0)
{
delaystart2 = 1; //1sec
a=1;
}
if(delaydone2)
{
delaystart2 = 0;
}
if (position==2 && metka==0 && delaydone2==0 )
{
delaystart3 = 1; //2sec

if(delaydone3)
{
delaystart3 = 0;
toolchanged = 1; //инструмент готов на чпу
e=0;
a=0;
}
}
}
}


break;

case 3:

if(position !=3 )
{
moto = 1; toolchanged = 0; //вперед
}


if (position==3 && metka==1 && e==0) //реверс
{
delaystart = 1; //0,15 sec антидребезг
if(delaydone==1)
{
delaystart = 0;
if (position==3 && metka==1)
{ moto = 0; e=1; }

}
}

if (e==1)
{
if (position==3 && metka==0 ) //проверка после реверса
{
if (a==0)
{
delaystart2 = 1; //1sec
a=1;
}
if(delaydone2)
{
delaystart2 = 0;
}
if (position==3 && metka==0 && delaydone2==0 )
{
delaystart3 = 1; //2sec

if(delaydone3)
{
delaystart3 = 0;
toolchanged = 1; //инструмент готов на чпу
e=0;
a=0;
}
}
}
}


break;

case 4:

if(position !=4 )
{
moto = 1; toolchanged = 0; //вперед
}


if (position==4 && metka==1 && e==0) //реверс
{
delaystart = 1; //0,15 sec антидребезг
if(delaydone==1)
{
delaystart = 0;
if (position==4 && metka==1)
{ moto = 0; e=1; }

}
}

if (e==1)
{
if (position==4 && metka==0 ) //проверка после реверса
{
if (a==0)
{
delaystart2 = 1; //1sec
a=1;
}
if(delaydone2)
{
delaystart2 = 0;
}
if (position==4 && metka==0 && delaydone2==0 )
{
delaystart3 = 1; //2sec

if(delaydone3)
{
delaystart3 = 0;
toolchanged = 1; //инструмент готов на чпу
e=0;
a=0;
}
}
}
}


break;

case 5:

if(position !=5 )
{
moto = 1; toolchanged = 0; //вперед
}


if (position==5 && metka==1 && e==0) //реверс
{
delaystart = 1; //0,15 sec антидребезг
if(delaydone==1)
{
delaystart = 0;
if (position==5 && metka==1)
{ moto = 0; e=1; }

}
}

if (e==1)
{
if (position==5 && metka==0 ) //проверка после реверса
{
if (a==0)
{
delaystart2 = 1; //1sec
a=1;
}
if(delaydone2)
{
delaystart2 = 0;
}
if (position==5 && metka==0 && delaydone2==0 )
{
delaystart3 = 1; //2sec

if(delaydone3)
{
delaystart3 = 0;
toolchanged = 1; //инструмент готов на чпу
e=0;
a=0;
}
}
}
}


break;

case 6:

if(position !=6 )
{
moto = 1; toolchanged = 0; //вперед
}


if (position==6 && metka==1 && e==0) //реверс
{
delaystart = 1; //0,15 sec антидребезг
if(delaydone==1)
{
delaystart = 0;
if (position==6 && metka==1)
{ moto = 0; e=1; }

}
}

if (e==1)
{
if (position==6 && metka==0 ) //проверка после реверса
{
if (a==0)
{
delaystart2 = 1; //1sec
a=1;
}
if(delaydone2)
{
delaystart2 = 0;
}
if (position==6 && metka==0 && delaydone2==0 )
{
delaystart3 = 1; //2sec

if(delaydone3)
{
delaystart3 = 0;
toolchanged = 1; //инструмент готов на чпу
e=0;
a=0;
}
}
}
}


break;

}
}

loadrt revolverka
addf revolverka servo-thread

loadrt timedelay count=4
addf timedelay.0 servo-thread
setp timedelay.0.on-delay 0.15

net delaystart timedelay.0.in <= revolverka.delaystart
net delaydone timedelay.0.out => revolverka.delaydone


addf timedelay.2 servo-thread
setp timedelay.2.on-delay 1

net delaystart2 timedelay.2.in <= revolverka.delaystart2
net delaydone2 timedelay.2.out => revolverka.delaydone2

addf timedelay.3 servo-thread
setp timedelay.3.on-delay 2

net delaystart3 timedelay.3.in <= revolverka.delaystart3
net delaydone3 timedelay.3.out => revolverka.delaydone3







net tool-change iocontrol.0.tool-change => revolverka.toolchange
net tool-changed iocontrol.0.tool-changed <= revolverka.toolchanged
net tool-number iocontrol.0.tool-prep-number => revolverka.toolnumber
net tool-oldnumber iocontrol.0.tool-number => revolverka.old
net tool-prepare-loopback iocontrol.0.tool-prepare => iocontrol.0.tool-prepared

net moto revolverka.moto => parport.0.pin-06-out

net opto1 revolverka.opto1 <= parport.0.pin-11-in
net opto2 revolverka.opto2 <= parport.0.pin-12-in
net opto3 revolverka.opto3 <= parport.0.pin-13-in
net metka revolverka.metka <= parport.0.pin-15-in