Have my tool changer working perfectly now under control from KMotionCNC.
http://81.138.85.180/~jim_cliff11/20...O-00000009.mp4
Took me the best part of a week to write the code and an immense amount of reading, but I got there eventually
If it helps anyone else my ToolPosChange.C code is below. It's probably fairly primitive, and there's probably more efficient ways to code it. But it does the job for me.
Code:
#include "KMotionDef.h"
//Define input pins to macros
#define IN_STROBE 132
#define IN_TEMP 134
#define IN_PROX 133
#define IN_BIT1 128
#define IN_BIT2 129
#define IN_BIT3 130
#define IN_BIT4 131
#define OUT_CW 154
#define OUT_CCW 153
//Define function prototypes
void CurEncoderValues(void);
void CurToolNo(void);
void ReqToolNo(void);
void ReqEncoderValues(void);
void ReqRotation(void);
void Actuate(void);
//Declare variables for current position
int intCurBit1;
int intCurBit2;
int intCurBit3;
int intCurBit4;
int intCurStrobe;
int intCurToolNo;
int intCurBin;
//Declare variables for new position
int intReqBit1;
int intReqBit2;
int intReqBit3;
int intReqBit4;
int intReqStrobe;
int intReqToolNo;
int intReqBin;
//Declare variables for control
int intDir;
int intProx;
int intTurretActive;
main()
{
//Disable limit switches in main INIT program (thread 1)
intTurretActive = 1;
//Move X axis to safe position
//Find current
CurEncoderValues();
CurToolNo();
//Find required
ReqToolNo();
ReqEncoderValues();
//Update console
printf("\nCurrent tool = %d at %d%d%d%d%d\n", intCurToolNo,intCurBit1,intCurBit2,intCurBit3,intCurBit4,intCurStrobe);
printf("Required tool = %d at %d%d%d%d%d\n", intReqToolNo,intReqBit1,intReqBit2,intReqBit3,intReqBit4,intReqStrobe);
//Determine required rotation
ReqRotation();
//Actuate turret in required direction if thermal relay OK
Actuate();
//Re-enable limit switches in main INIT program
intTurretActive = 0;
}
//Actuate turret
void Actuate(void)
{
if (intReqToolNo != intCurToolNo) {
SetBit(152);
intProx = ReadBit(IN_PROX);
while(intCurBit1 != intReqBit1 || intCurBit2 != intReqBit2 || intCurBit3 != intReqBit3 || intCurBit4 != intReqBit4 || intCurStrobe != intReqStrobe || intProx != 1) {
SetBit(intDir);
CurEncoderValues();
CurToolNo();
intProx = ReadBit(IN_PROX);
}
ClearBit(intDir);
ClearBit(152);
printf("Tool change complete\n");
} else {
printf("Tool already in position\n");
}
}
//Determine required rotation for least amount of moves to required position - Set bit No
void ReqRotation(void)
{
int c,r,cw,cc,rr,ccw;
for(c = intCurToolNo, r = intReqToolNo, cw = 0; c != r; cw++) {
c++;
if (c == 9) { c = 1 ;}
}
printf("No of positions CW = %d\n", cw);
for(cc = intCurToolNo, rr = intReqToolNo, ccw = 0; cc != rr; ccw++) {
cc--;
if (cc == 0) { cc = 8 ;}
}
printf("No of positions CCW = %d\n", ccw);
if (cw < ccw) { intDir = OUT_CW; } else if (ccw < cw) { intDir = OUT_CCW; }
printf("Rotation of turret will be = %d\n", intDir);
}
//Required encoder values for new tool position
void ReqEncoderValues(void)
{
switch(intReqToolNo) {
case 1 :
intReqBit1 = 1;
intReqBit2 = 0;
intReqBit3 = 0;
intReqBit4 = 0;
intReqStrobe = 1;
break;
case 2 :
intReqBit1 = 0;
intReqBit2 = 1;
intReqBit3 = 0;
intReqBit4 = 0;
intReqStrobe = 1;
break;
case 3 :
intReqBit1 = 1;
intReqBit2 = 1;
intReqBit3 = 0;
intReqBit4 = 0;
intReqStrobe = 1;
break;
case 4 :
intReqBit1 = 0;
intReqBit2 = 0;
intReqBit3 = 1;
intReqBit4 = 0;
intReqStrobe = 1;
break;
case 5 :
intReqBit1 = 1;
intReqBit2 = 0;
intReqBit3 = 1;
intReqBit4 = 0;
intReqStrobe = 1;
break;
case 6 :
intReqBit1 = 0;
intReqBit2 = 1;
intReqBit3 = 1;
intReqBit4 = 0;
intReqStrobe = 1;
break;
case 7 :
intReqBit1 = 1;
intReqBit2 = 1;
intReqBit3 = 1;
intReqBit4 = 0;
intReqStrobe = 1;
break;
case 8 :
intReqBit1 = 0;
intReqBit2 = 0;
intReqBit3 = 0;
intReqBit4 = 1;
intReqStrobe = 1;
break;
default :
intReqBit1 = 1;
intReqBit2 = 0;
intReqBit3 = 0;
intReqBit4 = 0;
intReqStrobe = 1;
}
}
//What is demanded tool number
void ReqToolNo(void)
{
intReqToolNo = persist.UserData[9];
}
//Check encoder values
void CurEncoderValues(void)
{
intCurBit1 = ReadBit(IN_BIT1);
intCurBit2 = ReadBit(IN_BIT2);
intCurBit3 = ReadBit(IN_BIT3);
intCurBit4 = ReadBit(IN_BIT4);
intCurStrobe = ReadBit(IN_STROBE);
}
//determine current tool number
void CurToolNo(void)
{
if ((intCurBit1 == 1) && (intCurBit2 == 0) && (intCurBit3 == 0) && (intCurBit4 == 0) && (intCurStrobe == 1)) {
intCurToolNo = 1; }
else if ((intCurBit1 == 0) && (intCurBit2 == 1) && (intCurBit3 == 0) && (intCurBit4 == 0) && (intCurStrobe == 1)) {
intCurToolNo = 2; }
else if ((intCurBit1 == 1) && (intCurBit2 == 1) && (intCurBit3 == 0) && (intCurBit4 == 0) && (intCurStrobe == 1)) {
intCurToolNo = 3; }
else if ((intCurBit1 == 0) && (intCurBit2 == 0) && (intCurBit3 == 1) && (intCurBit4 == 0) && (intCurStrobe == 1)) {
intCurToolNo = 4; }
else if ((intCurBit1 == 1) && (intCurBit2 == 0) && (intCurBit3 == 1) && (intCurBit4 == 0) && (intCurStrobe == 1)) {
intCurToolNo = 5; }
else if ((intCurBit1 == 0) && (intCurBit2 == 1) && (intCurBit3 == 1) && (intCurBit4 == 0) && (intCurStrobe == 1)) {
intCurToolNo = 6; }
else if ((intCurBit1 == 1) && (intCurBit2 == 1) && (intCurBit3 == 1) && (intCurBit4 == 0) && (intCurStrobe == 1)) {
intCurToolNo = 7; }
else if ((intCurBit1 == 0) && (intCurBit2 == 0) && (intCurBit3 == 0) && (intCurBit4 == 1) && (intCurStrobe == 1)) {
intCurToolNo = 8; }
}
Tom, I do have one question though......
Ideally, I would like to pass the variable intTurretActive from the above code across to my Init program in thread 1 (which is running continuously). The IF statement in my init program which controls the safety circuit will then have an addition condition:
Code:
//Monitor for E-stop, mains pwr, drive error signals if (ReadBit(136) == 0 || ReadBit(137) == 0 || ReadBit(139) == 0) {
if (ch0->Enable) DisableAxis(0);
if (ch1->Enable) DisableAxis(1);
if (ch2->Enable) DisableAxis(2);
ClearBit(159);
ClearBit(152);
ClearBit(153);
ClearBit(154);
//Monitor for limit switches (unless during tool turret rotation)
} else if (ReadBit(138) == 0) {
if (intTurretActive == 1) {
//Limit switches in-operable
} else {
if (ch0->Enable) DisableAxis(0);
if (ch1->Enable) DisableAxis(1);
if (ch2->Enable) DisableAxis(2);
ClearBit(159);
}
}
}
My objective is: When the tool turret is active, limit switches are inactive. I will write code into my ToolPosChange.c that moves the Z axis back UNTIL it hits the far limit switch (all wired into same input anyway). Once this limit switch is hit, the turret can rotate safely without risk of collision. When rotation complete, Z will move back to the position it was previously in. intTurretActive will then == 0, which is passed back to the init in thread 1. The limit switches are then re-enabled and machining can continue.
Thanks,
Jim