Еще одна задачка, которую я осилил в K-FLOP это поиск угла заготовки по средствам проводного сканера (не сочтите за рекламу, сканер покупал тут - Проводной ЧПУ сканер). Нужно еще допилить код для поиска внутреннего угла и центра отверстия, но по сути алгоритмы очень схожи.
Полезная штука для поиска нуля (угол по идее чаще всего и есть ноль заготовки, или же точка привязки) при первоначальной установке, но для меня это стало спасением при повторной установке деталей с поворотом/переворотом. Можно конечно воспользоваться упорами и не мучиться с этим сканером, но так интереснее
Алгоритм выполнения в общих чертах такой:
1. Опускаемся по Z до касания заготовки,
2. Поднимаемся над заготовкой, отъезжаем назад на 10мм по X и опускаемся на 5мм.,
3. Касаемся края заготовки,
4. Уезжаем начальную точку измерения,
5. То же самое проделываем по оси Y,
6. Делаем компенсацию диаметра шарика сенсора и едем точно в угол заготовки.
Вот видео самого процесса:
http://www.youtube.com/watch?v=W5GEOwiUeR0
А вот код, под управлением которого работает данный алгоритм (прошу прощения за коменты в коде на транслите, но компилятор k-flop не умеет печатать русские буквы, а писать коменты в отдельном редакторе не очень удобно), есть несколько идей по исключению некоторых лишних перемещений, но пока так:
Код: Выделить всё
#include "KMotionDef.h"
int DoPC(int cmd);
int DoPCInt(int cmd, int i);
#define GATH_OFF 0 // define the offset into the Gather buffer where strings are passed
main()
{
int Answer;
double *pD = (double *)persist.UserData;
double PZ0,PZ1,PZ2,PX0,PX1,PX2,PX3,PY0,PY1,PY2,PY3;
// PZ0 - tochka kasania skanerom zagotovki po osi Z
// PZ1 - tochka na 2mm nad zagotovkoy
// PZ2 - tochka na 5mm nizhe tochki PZ1 (na 3mm nizhe zagotovki)
// PX0 - tochka na osi X nachala izmerenia
// PX1 - tochka na 10mm nazad ot nachal'noy tochki X PX0
// PX2 - tochka kraiya zagotovki po osi X bez kompensacii diametra sharika sensora
// PX3 - tochka kraiya zagotovki po osi X s kompensaciey diametra sharika sensora
// PY0 - tochka na osi Y nachala izmerenia
// PY1 - tochka na 10mm nazad ot nachal'noy tochki Y PY0
// PY2 - tochka kraiya zagotovki po osi Y bez kompensacii diametra sharika sensora
// PY3 - tochka kraiya zagotovki po osi Y s kompensaciey diametra sharika sensora
//Opuskaemsia skanerom do kasania zagotovki, podnimaem os' Z na 2mm nad zagotovkoy i zapominaem polozhenie po osiam X,Y,Z
Jog(2,-800); // opuskaem os' Z vniz do kasania zagotovki
while (ReadBit(4)); // zhdem kasania i srabativania sensora s bit-4
Jog(2,0); // ostanavlivaem os' Z
PZ0 = chan[2].Dest; // zapisivaem polozhenie osi Z v peremennuyu PZ0
Delay_sec(0.3); // zhdem 0.3 sec
PZ1=PZ0+2564.8818; // peremennoy PZ0 dobavlayem 2mm (2564.8818 impulsov) i prisvevaem peremennoy PZ1
MoveAtVel(2,PZ1,25000); // peremeshaem os' Z na polozhenie PZ1
while (!CheckDone(2)); // zhdem okonchania dvizhenia osi Z
Delay_sec(0.3); // zhdem 0.3 sec
PX0 = chan[0].Dest; // zapisivaem polozhenie osi X v peremennuyu PX0 - tochka nachala izmerenia X
PY0 = chan[1].Dest; // zapisivaem polozhenie osi Y v peremennuyu PY0 - tochka nachala izmerenia Y
//Ot'ezhaem na 10mm nazad po osi X, opuskaem os' Z na 5mm vniz, edem po X vpered dokasaia sensorom zagotovki i zapominaem tochku osi X - kray zagotovki
PX1=PX0-12824.4094; // peremennoy PX0 otnimaem 10mm (12824.4094 inpulsov) i prisvevaem peremennoy PX1
MoveAtVel(0,PX1,25000); // os' X peremeshaem v polozhenie PX1
while (!CheckDone(0)); // zhdem okonchania dvizhenia osi X
Delay_sec(0.3); // zhdem 0.3 sec
PZ2=PZ1-6412.2047; // ot peremennoy PZ1 otnimaem 5mm (6412.2047 impulsov) i prisvaevaem peremennoy PZ2
MoveAtVel(2,PZ2,25000); // peremeshaem os' Z v polozhenie PZ2
while (!CheckDone(2)); // zhdem okonchania dvizhenia osi Z
Jog(0,800); // peremeshaem os' X vpered do kasania zagotovki
while (ReadBit(4)); // zhdem kasania i srabativania sensora s bit-4
Jog(0,0); // ostanavlivaem os' X
Delay_sec(0.3); // zhdem 0.3 sec
PX2 = chan[0].Dest; // zapisivaem polozhenie osi X na krayu zagotovki
//Ot'ezhaem nazad po X ot zagotovki na 10mm, podnimaem os' Z na 2mm nad zagotovkoy i edem po osi X d tochku nachala izmerenia X
MoveAtVel(0,PX1,25000); // os' X peremeshaem v polozhenie PX1
while (!CheckDone(0)); // zhdem okonchania dvizhenia osi X
MoveAtVel(2,PZ1,25000); // peremeshaem os' Z v polozhenie PZ1
while (!CheckDone(2)); // zhdem okonchania dvizhenia osi Z
Delay_sec(0.3); // zhdem 0.3 sec
MoveAtVel(0,PX0,25000); // peremeshaem os' X v polozhenie PX0
while (!CheckDone(0)); // zhdem okonchania dvizhenia osi X
Delay_sec(0.3); // zhdem 0.3 sec
//Ot'ezhaem na 10mm nazad po osi Y, opuskaem os' Z na 5mm vniz, edem po Y vpered dokasaia sensorom zagotovki i zapominaem tochku osi Y - kray zagotovki
PY1=PY0-12824.4094; // peremennoy PY0 otnimaem 10mm (12824.4094 inpulsov) i prisvevaem peremennoy PY1
MoveAtVel(1,PY1,25000); // os' Y peremeshaem v polozhenie PY1
while (!CheckDone(1)); // zhdem okonchania dvizhenia osi Y
Delay_sec(0.3); // zhdem 0.3 sec
MoveAtVel(2,PZ2,25000); // peremeshaem os' Z v polozhenie PZ2
while (!CheckDone(2)); // zhdem okonchania dvizhenia osi Z
Jog(1,800); // peremeshaem os' X vpered do kasania zagotovki
while (ReadBit(4)); // zhdem kasania i srabativania sensora s bit-4
Jog(1,0); // ostanavlivaem os' X
Delay_sec(0.3); // zhdem 0.3 sec
PY2 = chan[1].Dest; // zapisivaem polozhenie osi X na krayu zagotovki
//Ot'ezhaem nazad po Y ot zagotovki na 10mm, podnimaem os' Z na 2mm nad zagotovkoy i edem po osi X d tochku nachala izmerenia X
MoveAtVel(1,PY1,25000); // os' X peremeshaem v polozhenie PX1
while (!CheckDone(1)); // zhdem okonchania dvizhenia osi X
MoveAtVel(2,PZ1,25000); // peremeshaem os' Z v polozhenie PZ1
while (!CheckDone(2)); // zhdem okonchania dvizhenia osi Z
Delay_sec(0.3); // zhdem 0.3 sec
MoveAtVel(1,PY0,25000); // peremeshaem os' X v polozhenie PX0
while (!CheckDone(1)); // zhdem okonchania dvizhenia osi X
Delay_sec(0.3); // zhdem 0.3 sec
//Raschitivaem tochki kraev zagotovki s kompensiciey diametra sharika sensora
PX3=PX2+1025.9527; // k tochke kasania sensora po X pribavliaem 0.8mm (diametr sharika sensora 1.6mm)
PY3=PY2+1025.9527; // k tochke kasania sensora po Y pribavliaem 0.8mm (diametr sharika sensora 1.6mm)
//Edem v ugol zagotovi
MoveAtVel(0,PX3,25000); // os' X peremeshaem v polozhenie PX3
while (!CheckDone(0)); // zhdem okonchania dvizhenia osi X
MoveAtVel(1,PY3,25000); // os' Y peremeshaem v polozhenie PY3
while (!CheckDone(1)); // zhdem okonchania dvizhenia osi Y
//Prisvaivaem uglu polozhenie X=0, Y=0
DoPCFloat(PC_COMM_SET_X,0);
DoPCFloat(PC_COMM_SET_Y,0);
Delay_sec(0.1);
}
//Kod vvoda dannih ot DoPCFloat
int SetVars(int varoff, int n, int poff)
{
persist.UserData[PC_COMM_PERSIST+2] = n; // number of elements
persist.UserData[PC_COMM_PERSIST+3] = poff; // persist offset (doubles)
return DoPCInt(PC_COMM_SET_VARS,varoff); // Var index and Cmd
}
int GetVars(int varoff, int n, int poff)
{
persist.UserData[PC_COMM_PERSIST+2] = n; // number of elements
persist.UserData[PC_COMM_PERSIST+3] = poff; // persist offset (doubles)
return DoPCInt(PC_COMM_GET_VARS,varoff); // Var index and Cmd
}
#define GATH_OFF 0 // define the offset into the Gather buffer where strings are passed
// Trigger a message box on the PC to be displayed
// defines for MS Windows message box styles and Operator
// response IDs are defined in the KMotionDef.h file
int MsgBox(char *s, int Flags)
{
char *p=(char *)gather_buffer+GATH_OFF*sizeof(int);
int i;
do // copy to gather buffer w offset 0
{
*p++ = *s++;
}while (s[-1]);
persist.UserData[PC_COMM_PERSIST+2] = Flags; // set options
DoPCInt(PC_COMM_MSG,GATH_OFF);
return persist.UserData[PC_COMM_PERSIST+3];
}
// put the MDI string (Manual Data Input - GCode) in the
// gather buffer and tell the App where it is
int MDI(char *s)
{
char *p=(char *)gather_buffer+GATH_OFF*sizeof(int);
int i;
do // copy to gather buffer w offset 0
{
*p++ = *s++;
}while (s[-1]);
// issue the command an wait till it is complete
// (or an error - such as busy)
return DoPCInt(PC_COMM_MDI,GATH_OFF);
}
// Put a Float as a parameter and pass the command to the App
int DoPCFloat(int cmd, float f)
{
int result;
persist.UserData[PC_COMM_PERSIST+1] = *(int*)&f;
return DoPC(cmd);
}
// Put an integer as a parameter and pass the command to the App
int DoPCInt(int cmd, int i)
{
int result;
persist.UserData[PC_COMM_PERSIST+1] = i;
return DoPC(cmd);
}
// Pass a command to the PC and wait for it to handshake
// that it was received by either clearing the command
// or changing it to a negative error code
int DoPC(int cmd)
{
int result;
persist.UserData[PC_COMM_PERSIST]=cmd;
do
{
WaitNextTimeSlice();
}while (result=persist.UserData[PC_COMM_PERSIST]>0);
//printf("Result = %d\n",result);
return result;
}