I added your code to my Reference and Tool change programs. In the reference program, after the limit switch is triggered, carousel is supposed to rotate to current tool. But it never stops rotating. When rotating, carousel rotates one pocket at a time and pauses.
When trying to execute the Tool change code, carousel will rotate about 3 or 4(cant really tell by watching) pockets and pause.And just keeps repeating this, never stops.
Here is both complete programs.
ToolChangerReferenceV3
Code:
#include "KMotionDef.h"
#define TMP 10 // which spare persist to use to transfer data
#include "KflopToKMotionCNCFunctions.c"
#define Z 2
//V3 Servo Driven
main()
{
ch5->InputMode = ENCODER_MODE;
ch5->OutputMode = DAC_SERVO_MODE;
ch5->Vel = 20500;
ch5->Accel = 25000;
ch5->Jerk = 250000;
ch5->P = 40;
ch5->I = 0.005;
ch5->D = 0;
ch5->FFAccel = 0;
ch5->FFVel = 0.4;
ch5->MaxI = 800;
ch5->MaxErr = 200;
ch5->MaxOutput = 1950;
ch5->DeadBandGain = 1;
ch5->DeadBandRange = 0;
ch5->InputChan0 = 5;
ch5->InputChan1 = 5;
ch5->OutputChan0 = 5;
ch5->OutputChan1 = 11;
ch5->MasterAxis = -1;
ch5->LimitSwitchOptions = 0x100;
ch5->LimitSwitchNegBit = 0;
ch5->LimitSwitchPosBit = 0;
ch5->SoftLimitPos = 1e+30;
ch5->SoftLimitNeg = -1e+30;
ch5->InputGain0 = -1;
ch5->InputGain1 = 1;
ch5->InputOffset0 = 0;
ch5->InputOffset1 = 0;
ch5->OutputGain = 1;
ch5->OutputOffset = 0;
ch5->SlaveGain = 1;
ch5->BacklashMode = BACKLASH_OFF;
ch5->BacklashAmount = 0;
ch5->BacklashRate = 0;
ch5->invDistPerCycle = 1;
ch5->Lead = 0;
ch5->MaxFollowingError = 20000;
ch5->StepperAmplitude = 20;
ch5->iir[0].B0 = 1;
ch5->iir[0].B1 = 0;
ch5->iir[0].B2 = 0;
ch5->iir[0].A1 = 0;
ch5->iir[0].A2 = 0;
ch5->iir[1].B0 = 1;
ch5->iir[1].B1 = 0;
ch5->iir[1].B2 = 0;
ch5->iir[1].A1 = 0;
ch5->iir[1].A2 = 0;
ch5->iir[2].B0 = 1;
ch5->iir[2].B1 = 0;
ch5->iir[2].B2 = 0;
ch5->iir[2].A1 = 0;
ch5->iir[2].A2 = 0;
// Tool Changer IN
SetBit(62); // TC_IN solenoid
Delay_sec(0.7);
while (!ReadBit(1035)) // Check TC_IN limit switch
ClearBit(62); // TC_IN solenoid
// Move Tool Changer UP
SetBit(60); // TC_UP Solenoid
Delay_sec(0.9);
while (!ReadBit(1037)) // Wait for TC_UP Limit Switch
ClearBit(60); //TC_UP solenoid
// Rotate Carousel to LimitSwitch
DisableAxis(5);
Zero(5);
EnableAxisDest(5, 0.0);
Jog(5, 2000); // start moving
Delay_sec(2.5); //??What if Limit Switch already Triggered??
while (!ReadBit(1032)); // wait for Carousel Limit switch
Jog(5, 0); // StopMotion
while (!CheckDone(5));
Delay_sec(.5);
DisableAxis(5);
Zero(5);
EnableAxisDest(5, 0.0);
Delay_sec(.5);
Move(5, 435);
while (!CheckDone(5));
Delay_sec(.5);
DisableAxis(5);
Zero(5);
EnableAxisDest(5, 0.0);
Delay_sec(.5);
FILE *f;
char s[256];
double CTool = 0;
int result;
persist.UserData[157] = 16;
printf("Current tool=%f\n", CTool);
// Send MDI code to kmotion to display proper tool
char s[80];
sprintf(s, "T%d", persist.UserData[157]);
MDI(s); //send the T word
Delay_sec(0.5);
FILE *f;
char s[256];
double CTool = 0;
int result;
f = fopen("c:\\KMotionFiles\\KFlopData.txt", "rt");
if (!f)
{
printf("Unable to open file\n");
return;
}
result = fscanf(f, "%lf", &CTool);
fclose(f);
persist.UserData[157] = CTool;
printf("Current tool=%f\n", CTool);
// Send MDI code to kmotion to display proper tool
char s[80];
sprintf(s, "T%d", persist.UserData[157]);
MDI(s); //send the T word
// 1. Determine how many pockets to rotate
int pockets = persist.UserData[157]; // pockets = requested tool - current tool
if (pockets == 0)
return 0; // No toolchange needed, End
if (pockets >= 8)
pockets = -16 + pockets; // If more then halfway CW, reverse
if (pockets <= -8)
pockets = 16 + pockets; // If more then halfway CCW, reverse
persist.UserData[161] = 1; // Toolchanger active disable feedhold button
ResumeCoordinatedMotion(); // Clear feedhold
// 9. Rotate Carousel
while (pockets >= 1) // Rotate Turret CW whatever number of pockets
{
MoveRel(5, pockets * 1682.69);
while (!CheckDone(5)); // wait till finished
}
while (pockets <= -1) // Rotate Turret CCW whatever number of pockets
{
MoveRel(5, -pockets * 1682.69);
while (!CheckDone(5)); // wait till finished
}
ClearBit(62); //TC_IN Solenoid
ClearBit(60); //TC_UP Solenoid
ClearBit(48); //ZeroServoCommand to VFD
ClearBit(156); //SpindleOrientateProx minusVolt
ClearBit(147); //SpindleCW
ClearBit(148); //SpindleCCW
}
M6ToolChangeV2
Code:
#include "KMotionDef.h"
#define TMP 10 // which spare persist to use to transfer data
#include "KflopToKMotionCNCFunctions.c"
#define Z 2
//V2, Servo Driven
main()
{
if (!ReadBit(137))
return 0; // if in Estop, Exit program
// 1. Determine how many pockets to rotate
int pockets = persist.UserData[6] - persist.UserData[157]; // pockets = requested tool - current tool
if (pockets == 0)
return 0; // No toolchange needed, End
if (pockets >= 8)
pockets = -16 + pockets; // If more then halfway CW, reverse
if (pockets <= -8)
pockets = 16 + pockets; // If more then halfway CCW, reverse
persist.UserData[161] = 1; // Toolchanger active disable feedhold button
ResumeCoordinatedMotion(); // Clear feedhold
// 2. Turn off Flood Coolant if left on
int coolanton = 0;
if (ReadBit(157)) // if flood coolant on
{
coolanton = 1; // set variable to 1
ClearBit(157); // Flood coolant off
Delay_sec(.2); // delay
}
// 3. Turn off spindle
if (ReadBit(147) || ReadBit(148)) //check if spindle is running forward or reverse
{
ClearBit(147); // turn off spindle clockwise
ClearBit(148); // turn off spindle counterclockwise
while (fast_fabs(Spindle.TrueSpeedRPS) * 60.0 > 0.0); //wait untiless than 5RPM
Delay_sec(.2);
}
// 4. Move to toolchange position Z-1.01inches from Reference Position
{
Move(Z, -20528);
while (!CheckDone(2))
WaitNextTimeSlice(); // wait until we are stopped
}
// 5. Orientate
if (!ReadBit(1035)) // if toolchanger slide is not home estop
{
ClearBit(137); // E-stop
printf("E-stop Toolchanger not Home");
}
{
persist.UserData[155] = 1; // Enable orientate speed override
SetBit(156); // Kanalog Switch4 to turn on SpindleOrientateProxSensor
Delay_sec(0.1); // Delay 0.1s
DAC(7, -16); // set motor rpm approx 30
SetBit(147); // spindle on CW
Delay_sec(0.2); // wait for spindle to reach speed
while (!ReadBit(1038))
WaitNextTimeSlice(); // wait for SpindleOrientateProx signal
SetBit(48); // Signal to VFD, set VFD to ZeroServoCommand
Delay_sec(0.90); // Delay for spindle to stop at orientation that is set in VFD parameter
}
{
persist.UserData[155] = 0; // Spindle speed normal
Delay_sec(0.75); // wait before checking if successfull
}
while (!ReadBit(1035) || !ReadBit(1037))
WaitNextTimeSlice(); // Check TC is at home position.
// 6. Tool Chnager OUT
SetBit(63); // TC_OUT solenoid
Delay_sec(1.1);
ClearBit(63); //TC_OUT solenoid
double TF = Time_sec() + 2; // timeout in 2 seconds
while (!ReadBit(1034)) // Wait for TC_OUT limit switch
{
WaitNextTimeSlice();
if (Time_sec() > TF) // Timed out?
{
if (MsgBox("TC OUT Not Extended. Halt?", MB_YESNO | MB_ICONEXCLAMATION) == IDYES)
{
DoPC(PC_COMM_HALT);
persist.UserData[161] = 0; // toolchanger finished
return 0; //exit program
}
else
{
TF = Time_sec() + 3; // continue and timeout after 3 seconds
}
}
}
// 7. Unclamp Tool
SetBit(59); //Tool Release Solenoid
Delay_sec(.6);
double TF = Time_sec() + 2; // timeout in 2 seconds
while (!ReadBit(1039)) // Wait for ToolReleaseProxSensor
{
WaitNextTimeSlice();
if (Time_sec() > TF) // Timed out?
{
if (MsgBox("Tool Release Failed. Halt?", MB_YESNO | MB_ICONEXCLAMATION) == IDYES)
{
DoPC(PC_COMM_HALT);
persist.UserData[161] = 0; // toolchanger finished
return 0; //exit program
}
else
{
TF = Time_sec() + 3; // continue and timeout after 3 seconds
}
}
}
// 8. Move Tool Changer DOWN
SetBit(61); // TC_DOWN Solenoid
Delay_sec(0.9);
ClearBit(61); //TC_DOWN solenoid
double TF = Time_sec() + 2; // timeout in 2 seconds
while (!ReadBit(1036)) // Wait for TC_DOWN Limit Switch
{
WaitNextTimeSlice();
if (Time_sec() > TF) // Timed out?
{
if (MsgBox("TC DOWN Failed. Halt?", MB_YESNO | MB_ICONEXCLAMATION) == IDYES)
{
DoPC(PC_COMM_HALT);
persist.UserData[161] = 0; // toolchanger finished
return 0; //exit program
}
else
{
TF = Time_sec() + 3; // continue and timeout after 3 seconds
}
}
}
// 9. Rotate Carousel
while (pockets >= 1) // Rotate Turret CW whatever number of pockets
{
MoveRel(5, pockets * 1682.69);
while (!CheckDone(5)); // wait till finished
}
while (pockets <= -1) // Rotate Turret CCW whatever number of pockets
{
MoveRel(5, -pockets * 1682.69);
while (!CheckDone(5)); // wait till finished
}
// 10. Move Tool Changer UP
SetBit(60); // TC_UP Solenoid
Delay_sec(0.9);
ClearBit(60); //TC_UP solenoid
double TF = Time_sec() + 2; // timeout in 2 seconds
while (!ReadBit(1037)) // Wait for TC_UP Limit Switch
{
WaitNextTimeSlice();
if (Time_sec() > TF) // Timed out?
{
if (MsgBox("TC UP Failed. Halt?", MB_YESNO | MB_ICONEXCLAMATION) == IDYES)
{
DoPC(PC_COMM_HALT);
persist.UserData[161] = 0; // toolchanger finished
return 0; //exit program
}
else
{
TF = Time_sec() + 3; // continue and timeout after 3 seconds
}
}
}
// 11. Clamp tool
ClearBit(59); //Tool Release Solenoid
double TF = Time_sec() + 2; // timeout in 2 seconds
while (ReadBit(1039)) // Wait for Tool Release Proximity Sensor
{
WaitNextTimeSlice();
if (Time_sec() > TF) // Timed out?
{
if (MsgBox("Tool Clamp Failed. Halt?", MB_YESNO | MB_ICONEXCLAMATION) == IDYES)
{
DoPC(PC_COMM_HALT);
persist.UserData[161] = 0; // toolchanger finished
return 0; //exit program
}
else
{
TF = Time_sec() + 3; // continue and timeout after 3 seconds
}
}
}
// 12. Tool Changer IN
SetBit(62); // TC_IN solenoid
Delay_sec(0.7);
ClearBit(62); // TC_IN solenoid
double TF = Time_sec() + 2; // timeout in 2 seconds
while (!ReadBit(1035)) // Check TC_IN limit switch
{
WaitNextTimeSlice();
if (Time_sec() > TF) // Timed out?
{
if (MsgBox("TC Not Home. Halt?", MB_YESNO | MB_ICONEXCLAMATION) == IDYES)
{
DoPC(PC_COMM_HALT);
persist.UserData[161] = 0; // toolchanger finished
return 0; //exit program
}
else
{
TF = Time_sec() + 3; // continue and timeout after 3 seconds
}
}
}
// 13. Set Variables
persist.UserData[157] = persist.UserData[6]; // Update current tool number
persist.UserData[161] = 0; // toolchanger finished
printf("Tool changed to T%d\n", persist.UserData[157]); // send message to console
// 14. Write current tool # disk
FILE *f;
char s[256];
float CTool = persist.UserData[157]; // s value from g code
f = fopen("c:\\KMotionFiles\\KFlopData.txt", "wt");
fprintf(s, "%f\n", CTool);
fclose(f);
// 15. Turn coolant back on
if (coolanton == 1)
SetBit(157); // if flood coolant was on, turn on again
ClearBit(156);
ClearBit(48);
ClearBit(147);
}