cnc-club.ru

Код: Выделить всё

#include "KMotionDef.h"

// T O O L   C H A N G E R   S E Q U E N C E
//
// define state that the tool changer may be in
enum {T_IDLE,T_WORKING};

#define TOOL_VAR 20        	// Tool changer desired new tool Var
#define TOOL_STATE_VAR 19  	// Tool changer state is saved globally in this Var
#define LAST_TOOL_VAR 18   	// Tool changer Last tool position is saved globally in this Var
#define ZHOME 0             // Absolute Position of Z Axis Home
#define ZTOOLPOS ******     // Absolute Position of Z Axis For Top Of Tool Slot

int *ChangerState 	= &persist.UserData[TOOL_STATE_VAR];
int *Tool     		= &persist.UserData[TOOL_VAR];
int *LastTool 		= &persist.UserData[LAST_TOOL_VAR];

double ToolTime;

// Returns Current Tool Number
int CurrentTool(void){
    int ToolNum = 0;
}
    if (ReadBit(140)){       
    return ToolNum;
}

// Moves Magazine To Specified Slot
void MoveMagazine(int tool){
	
    int delta = tool - CurrentTool();   // how far to move
    if (delta>10)  delta-=20;            // optimize direction
    if (delta<-10) delta+=20;
	printf("Moving Magazine %d Slots\n", delta);
    StepMagazine(delta);
    return;
}

// Moves Magazine By Specified Number Of Slots
void StepMagazine(int delta){

    if (!ReadBit(???) && (Pos(2) < -100)) ????????????????
}
    {
        printf("Error Spindle In Invalid Position\n");
        *ChangerState = T_IDLE;  // go idle

    } else {

        int i = 0;
        int dir;
        if (delta > 0){
            SetBit(148);
	While(ReadBit(141);
	if (ReadBit(141)){
	ClearBit(148);	
        {    
	dir = 1;
        } else {
            SetBit(149);
	While(ReadBit(141);
	if (ReadBit(141)){
	ClearBit(149);	
        { 
            dir = -1;
        }
        while (delta != i){
            while(ReadBit(140)){
                WaitNextTimeSlice();
            }
            while(!ReadBit(140)){
                WaitNextTimeSlice();
            }
            i += dir;
        }
        ClearBit(149);
        ClearBit(148);
        return;
    }
}

main()
{
    
	{
		/// T O O L  C H A N G E R  S T A R T
		
		if (*ChangerState == T_IDLE)
		{
			*ChangerState = T_WORKING;
			
			// Spindle stop
			Move(3,0);		// move a-axis to 0
				
			// Move Z Axis To TOOL Position
			Move(2, ZTOOLPOS);
			while (!CheckDone(2)) ;
				if (!chan[2].Enable) return 1;
			
			// Wait For Z Axis To Stop Moving
				while (!CheckDone(2)) ;
					if (!chan[2].Enable) return 1;

			if (*LastTool==0)
			{
				printf("Error Magazine Position Never Defined\n");
				*ChangerState = T_IDLE;  // go idle
				return;
			}
			else if (*Tool > 20 || *Tool < 1)
			{
				printf("Invalid Tool Number %d\n",*Tool);
				*ChangerState = T_IDLE;  // go idle
				return;
			}
			else if (*Tool == *LastTool)
			{
				printf("Requested Tool (#%d) Already In Position\n",*Tool);
				*ChangerState = T_IDLE;  // go idle
				return;
			}
			else
			{

				// Move Magazine To Last Tool Position If Not Already There
				if (CurrentTool() != *LastTool){
					MoveMagazine(*LastTool);
				}					

		                                             
				// Move Magazine into position of tool slot
				SetBit(144);
				While(ReadBit(136);
				if (ReadBit(136)){
					ClearBit(144);
				}

				// Unclamp Tool Освободить инструмент
				SetBit(145);
				While(ReadBit(137);
				if (ReadBit(137)){
					ClearBit(145);
				}
				printf("tool Unclamped\n");
			
				//Up Z-Axis
				MoveRel(2,150);		// 150 mm
				
				// Wait For Z Axis To Stop Moving
				while (!CheckDone(2)) ;
					if (!chan[2].Enable) return 1;

			
				// Move Magazine To Desired Tool Position
				MoveMagazine(*Tool);

				// Check If Current Tool Is Desired Tool
				if (CurrentTool() == *Tool)
				{
					printf("Tool Clamp\n");

					Move(2, ZTOOLPOS);
					while (!CheckDone(2)) ;
						if (!chan[2].Enable) return 1;

					// Clamp Tool
					SetBit(146);
					While(ReadBit(138);
					if (ReadBit(138)){
					ClearBit(146);
					
				} else {
				
					printf("Wrong Tool Slot - %d/n", CurrentTool());
					return;
					
				}

				// Move Magazine into home position
				SetBit(147);
				While(ReadBit(139);
				if (ReadBit(139)){
					ClearBit(147);
				
				// Move Z Axis To Home Position
				Move(2, ZHOME);
				while (!CheckDone(2)) ;
					if (!chan[2].Enable) return 1;
							
				printf("Tool Change Complete\n");
				*LastTool = *Tool;  // remember where we are
				*ChangerState = T_IDLE;
			}
		}
	}
}