#include "KMotionDef.h"
#define Zaxis 2
#define TMP 3
#include "KflopToKMotionCNCFunctions.c"
#define PC_COMM_PERSIST 100
#define N_PC_COMM_PERSIST 8
#define round(x) ((x)>=0?(long)((x)+0.5)long)((x)-0.5))
#define QA 40 // define to which IO bits the AB signals are connected
#define QB 41
#define AirCylHomePos 138 //Carousel air cylinder is at home position
#define AirCylToolPos 139 //Carousel air cylinder is at tool change position
#define AirCyl 156 // Activate Air Cylinder for tool change
#define ERes 1024 //Encoder Resolution, 10bits
#define DeltaD 102 //Decimal value to compensate for absolute encoder zero alignment
/*INPUTS and OUTPUTS*/
/*INPUTS*/
#define Index 137 //Indexing sensor to index the carousel position at each motor turn, CW or CCW
#define CarRef 25 //When active carousel is at reference position corresponding to pocket #1
#define J 1031 // Indexing sensor to control the indexing motor CW/CCW
#define K 1030 // Indexing sensor to control the indexing motor CW/CCW
#define G0 1046 // Konnect: 10bits for the gray code G1=>G10
#define G1 1047 // G0 stands for Gray and 0 the power factor => 2^0,..., G9 => 2^9
#define G2 1048
#define G3 1049
#define G4 1050
#define G5 1051
#define G6 1052
#define G7 1053
#define G8 1054
#define G9 1055
/*OUTPUTS*/
#define CarCW 154 //Carousel ++Pocket index
#define CarCCW 155 //Carousel --Pocket index
/*------------------------------
--Global variables declaration--
------------------------------*/
int x;
int index, CWPos, CCWPos;
int OldTool, ActTool, ActSlot, NewTool, PrevSlot, ReqSlot;
int Units, TWORD, HWORD, DWORD;
unsigned short int GRAY[10], BIN[10];
int i, j, k;
int CW(); //Moves the umbrella clockwise
int CCW(); //Moves the umbrella counterclockwise
int LeastTravel(); // Determines the least amount of travelling time to reach a specific pocket or tool with an umbrella type tool changer
unsigned int GrayToBinary16(); //Converts the Gray Code from an absolute encoder to a binary number (0-360 degrees)
int alpha; //Angular position of the umbrella
int ActPos, InvActPos; // Rounded results from the angular division
int ActSlotPos(int alpha, int NSlot, int ActPos); //Reads the actual data and converts it to a corresponding pocket number
float alphaRes = 1024.0f / 360.0f; //Resolution [bits/degrees]
float AngularPosition(unsigned short int D, float alphaRes); // Converts a binary number to an angular position
int NSlot = 10; // highest number tool slot in carousel
int main()
{
int ToolSlot, ToolID;
/*INIT*/ InitAux();
AddKonnect(0, &VirtualBits, VirtualBitsEx);
/*-------------------------------------
--------------Data Input---------------
-------------------------------------*/
ToolSlot = persist.UserData[TMP];
ToolID = persist.UserData[TMP+1];
printf("ToolSlot = %d, ToolID = %d\n", ToolSlot, ToolID);
//GetMiscSettings(&Units, &TWORD, &HWORD, &DWORD);
//printf("Units=%d T=%d H=%d D=%d\n", Units, TWORD, HWORD, DWORD);
//printf("The Next Tool is: T#%d\n", persist.UserData[TMP + 1] = TWORD);
ReqSlot = persist.UserData[TMP]; //Pocket# of the new Tool ID#
NewTool = persist.UserData[TMP + 1]; //Tool ID# needed
ActSlot = persist.UserData[TMP + 2]; //Actual Pocket#
ActTool = persist.UserData[TMP + 3]; //Actual Tool ID#
PrevSlot = persist.UserData[TMP + 4]; //Previous Pocket# used as temporary variable while changing tools
OldTool = persist.UserData[TMP + 5]; //Previous Tool ID# used as temporary variable while changing tools
printf("\n The actual slot position is:%d\n", ActSlot);
printf("The Old Tool is T#%d\t The Previous Slot is #%d\n", OldTool, PrevSlot);
printf("The Actual Tool is T#%d\t The Actual Slot is #%d\n", ActTool, ActSlot);
printf("The NewTool is T#%d\t The Requested Slot is #%d\n", NewTool, ReqSlot);
/*-------------------------------------
-----------Flow Sequence---------------
-------------------------------------*/
unsigned short int D;
D=GrayToBinary16();
AngularPosition(D, alphaRes);
ActSlotPos(alpha, NSlot, ActPos);
LeastTravel();
return 0;
}
|