Code:
#include "KMotionDef.h"
/*
Initialization and button handler functions
for the J325 Milling Machine.
This file executes in Thread 1
R. Bickle, ICS, 12/2016
x and y axes have 20,000 encoder counts per inch
z axis has 25,000 encoder counts per inch
spindle has 5600 encoder counts per revolution
*/
// Input Bits
#define FEEDHOLDBIT 19
#define CYCLESTARTBIT 16
#define ESTOP 136
#define HALTBIT 18
#define RESTARTBIT 17
#define CLAMP 142
#define UNCLAMP 141
//#define ZEROALLBIT 29
#define OVERTRAVEL 137
#define OVERLOAD 138
#define SPINDLE0SPEED 139
#define SPINDLEATSPEED 140
#define AIRPRESSOK 143
// Output Bits
#define DRIVE_ENABLE 152
#define SPINDLE_ENABLE 153
#define DRAWBAR_OPEN 154
// Set initial output state
ClearBit (DRIVE_ENABLE);
ClearBit (SPINDLE_ENABLE);
ClearBit (DRAWBAR_OPEN);
// function prototypes for compiler
int DoPC(int cmd);
int DoPCFloat(int cmd, float f);
int Debounce(int n, int *cnt, int *last, int *lastsolid);
void ButtonService (void);
// global state variables for switch debouncing
int flast=0,flastsolid=-1,fcount=0;
int clast=0,clastsolid=-1,ccount=0;
int elast=0,elastsolid=-1,ecount=0;
int elast2=0,elastsolid2=-1,ecount2=0;
int hlast=0,hlastsolid=-1,hcount=0;
int rlast=0,rlastsolid=-1,rcount=0;
int zlast=0,zlastsolid=-1,zcount=0;
int clamplast=0,clamplastsolid=-1,clampcount=0;
int unclamplast=0,unclamplastsolid=-1,unclampcount=0;
int Estop = 0;
main()
{
ch0->InputMode=ENCODER_MODE;
ch0->OutputMode=DAC_SERVO_MODE;
ch0->Vel=60000;
ch0->Accel=250000;
ch0->Jerk=5e+006;
ch0->P=8;
ch0->I=1e-006;
ch0->D=100;
ch0->FFAccel=0.00025;
ch0->FFVel=0.034;
ch0->MaxI=2000;
ch0->MaxErr=3500;
ch0->MaxOutput=2000;
ch0->DeadBandGain=1;
ch0->DeadBandRange=0;
ch0->InputChan0=0;
ch0->InputChan1=1;
ch0->OutputChan0=0;
ch0->OutputChan1=1;
ch0->MasterAxis=-1;
ch0->LimitSwitchOptions=0x12c;
ch0->LimitSwitchNegBit=137;
ch0->LimitSwitchPosBit=137;
ch0->SoftLimitPos=1e+009;
ch0->SoftLimitNeg=-1e+009;
ch0->InputGain0=-1;
ch0->InputGain1=1;
ch0->InputOffset0=0;
ch0->InputOffset1=0;
ch0->OutputGain=-1;
ch0->OutputOffset=0;
ch0->SlaveGain=1;
ch0->BacklashMode=BACKLASH_OFF;
ch0->BacklashAmount=0;
ch0->BacklashRate=0;
ch0->invDistPerCycle=1;
ch0->Lead=0;
ch0->MaxFollowingError=10000000;
ch0->StepperAmplitude=250;
ch0->iir[0].B0=1;
ch0->iir[0].B1=0;
ch0->iir[0].B2=0;
ch0->iir[0].A1=0;
ch0->iir[0].A2=0;
ch0->iir[1].B0=1;
ch0->iir[1].B1=0;
ch0->iir[1].B2=0;
ch0->iir[1].A1=0;
ch0->iir[1].A2=0;
ch0->iir[2].B0=0.0166094;
ch0->iir[2].B1=0.0332189;
ch0->iir[2].B2=0.0166094;
ch0->iir[2].A1=1.60679;
ch0->iir[2].A2=-0.673229;
ch1->InputMode=ENCODER_MODE;
ch1->OutputMode=DAC_SERVO_MODE;
ch1->Vel=60000;
ch1->Accel=250000;
ch1->Jerk=4e+006;
ch1->P=7;
ch1->I=1e-005;
ch1->D=72;
ch1->FFAccel=0.00023;
ch1->FFVel=0.0311;
ch1->MaxI=2000;
ch1->MaxErr=3500;
ch1->MaxOutput=2000;
ch1->DeadBandGain=1;
ch1->DeadBandRange=0;
ch1->InputChan0=1;
ch1->InputChan1=1;
ch1->OutputChan0=1;
ch1->OutputChan1=1;
ch1->MasterAxis=-1;
ch1->LimitSwitchOptions=0x12c;
ch1->LimitSwitchNegBit=137;
ch1->LimitSwitchPosBit=137;
ch1->SoftLimitPos=1e+009;
ch1->SoftLimitNeg=-1e+009;
ch1->InputGain0=-1;
ch1->InputGain1=1;
ch1->InputOffset0=0;
ch1->InputOffset1=0;
ch1->OutputGain=-1;
ch1->OutputOffset=0;
ch1->SlaveGain=1;
ch1->BacklashMode=BACKLASH_OFF;
ch1->BacklashAmount=0;
ch1->BacklashRate=0;
ch1->invDistPerCycle=1;
ch1->Lead=0;
ch1->MaxFollowingError=10000000;
ch1->StepperAmplitude=250;
ch1->iir[0].B0=1;
ch1->iir[0].B1=0;
ch1->iir[0].B2=0;
ch1->iir[0].A1=0;
ch1->iir[0].A2=0;
ch1->iir[1].B0=1;
ch1->iir[1].B1=0;
ch1->iir[1].B2=0;
ch1->iir[1].A1=0;
ch1->iir[1].A2=0;
ch1->iir[2].B0=0.016609;
ch1->iir[2].B1=0.033219;
ch1->iir[2].B2=0.016609;
ch1->iir[2].A1=1.60679;
ch1->iir[2].A2=-0.673229;
ch2->InputMode=ENCODER_MODE;
ch2->OutputMode=DAC_SERVO_MODE;
ch2->Vel=60000;
ch2->Accel=600000;
ch2->Jerk=1e+006;
ch2->P=0.8;
ch2->I=1e-005;
ch2->D=125;
ch2->FFAccel=0.000125;
ch2->FFVel=0.0294;
ch2->MaxI=2000;
ch2->MaxErr=3500;
ch2->MaxOutput=2000;
ch2->DeadBandGain=1;
ch2->DeadBandRange=0;
ch2->InputChan0=2;
ch2->InputChan1=1;
ch2->OutputChan0=2;
ch2->OutputChan1=1;
ch2->MasterAxis=-1;
ch2->LimitSwitchOptions=0x12c;
ch2->LimitSwitchNegBit=137;
ch2->LimitSwitchPosBit=137;
ch2->SoftLimitPos=1e+009;
ch2->SoftLimitNeg=-1e+009;
ch2->InputGain0=-1;
ch2->InputGain1=1;
ch2->InputOffset0=0;
ch2->InputOffset1=0;
ch2->OutputGain=-1;
ch2->OutputOffset=0;
ch2->SlaveGain=1;
ch2->BacklashMode=BACKLASH_OFF;
ch2->BacklashAmount=0;
ch2->BacklashRate=0;
ch2->invDistPerCycle=1;
ch2->Lead=0;
ch2->MaxFollowingError=10000000;
ch2->StepperAmplitude=250;
ch2->iir[0].B0=1;
ch2->iir[0].B1=0;
ch2->iir[0].B2=0;
ch2->iir[0].A1=0;
ch2->iir[0].A2=0;
ch2->iir[1].B0=1;
ch2->iir[1].B1=0;
ch2->iir[1].B2=0;
ch2->iir[1].A1=0;
ch2->iir[1].A2=0;
ch2->iir[2].B0=0.016609;
ch2->iir[2].B1=0.033219;
ch2->iir[2].B2=0.016609;
ch2->iir[2].A1=1.60679;
ch2->iir[2].A2=-0.673229;
ch4->InputMode=NO_INPUT_MODE;
ch4->OutputMode=DAC_SERVO_MODE;
ch4->Vel=0.5;
ch4->Accel=1;
ch4->Jerk=5;
ch4->P=0;
ch4->I=0;
ch4->D=0;
ch4->FFAccel=0;
ch4->FFVel=2047;
ch4->MaxI=2047;
ch4->MaxErr=1e+010;
ch4->MaxOutput=2047;
ch4->DeadBandGain=1;
ch4->DeadBandRange=0;
ch4->InputChan0=4;
ch4->InputChan1=1;
ch4->OutputChan0=4;
ch4->OutputChan1=1;
ch4->MasterAxis=-1;
ch4->LimitSwitchOptions=0x100;
ch4->LimitSwitchNegBit=0;
ch4->LimitSwitchPosBit=0;
ch4->SoftLimitPos=1e+009;
ch4->SoftLimitNeg=-1e+009;
ch4->InputGain0=1;
ch4->InputGain1=1;
ch4->InputOffset0=0;
ch4->InputOffset1=0;
ch4->OutputGain=-1;
ch4->OutputOffset=0;
ch4->SlaveGain=1;
ch4->BacklashMode=BACKLASH_OFF;
ch4->BacklashAmount=0;
ch4->BacklashRate=0;
ch4->invDistPerCycle=1;
ch4->Lead=0;
ch4->MaxFollowingError=10000000;
ch4->StepperAmplitude=250;
ch4->iir[0].B0=1;
ch4->iir[0].B1=0;
ch4->iir[0].B2=0;
ch4->iir[0].A1=0;
ch4->iir[0].A2=0;
ch4->iir[1].B0=1;
ch4->iir[1].B1=0;
ch4->iir[1].B2=0;
ch4->iir[1].A1=0;
ch4->iir[1].A2=0;
ch4->iir[2].B0=1;
ch4->iir[2].B1=0;
ch4->iir[2].B2=0;
ch4->iir[2].A1=0;
ch4->iir[2].A2=0;
// Enable the axes and set the destination
//EnableAxisDest(0,0);
//EnableAxisDest(1,0);
//EnableAxisDest(2,0);
EnableAxis (0);
EnableAxis (1);
EnableAxis (2);
EnableAxis (4);
// Set coordinates to x, y, z and a. A is disabled
DefineCoordSystem(0,1,2,-1);
// Configure input bits
//SetBitDirection (FEEDHOLDBIT, 0);
//SetBitDirection (CYCLESTARTBIT, 0);
//SetBitDirection (ESTOP, 0);
//SetBitDirection (HALTBIT, 0);
//SetBitDirection (RESTARTBIT, 0);
//SetBitDirection (CLAMP, 0);
//SetBitDirection (UNCLAMP, 0);
//SetBitDirection (ZEROALLBIT, 0);
//SetBitDirection (OVERTRAVEL, 0);
//SetBitDirection (OVERLOAD, 0);
//SetBitDirection (SPINDLE0SPEED, 0);
//SetBitDirection (SPINDLEATSPEED, 0);
//SetBitDirection (AIRPRESSOK, 0);
// Configure output bits
//SetBitDirection (DRAWBAR_OPEN, 1);
//SetBitDirection (DRIVE_ENABLE, 1);
//SetBitDirection (SPINDLE_ENABLE, 1);
// Print startup message to console
printf("Button Handler Start\n");
// Enable the axis drive
SetBit (DRIVE_ENABLE);
//Delay_sec (1);
//SetBit (SPINDLE_ENABLE);
// Continuous loop
while (1)
{
WaitNextTimeSlice(); // Delay until time slice
ButtonService (); // Handle Button Presses
}
}
// ************************************************************************
// Handle Buttons
void ButtonService ()
{
int result;
// Handle Drawbar Clamp
result = Debounce(ReadBit(CLAMP),&clampcount,&clamplast,&clamplastsolid);
if (result == 1)
{
// Clamp drawbar if job is not running, spindle at 0 speed
if (!JOB_ACTIVE && ReadBit(SPINDLE0SPEED))
{
ClearBit (DRAWBAR_OPEN);
printf("Drawbar Closed\n");
}
}
// Handle Drawbar Unclamp
result = Debounce(ReadBit(UNCLAMP),&unclampcount,&unclamplast,&unclamplastsolid);
if (result == 1)
{
// Clamp drawbar if job is not running, spindle at 0 speed
if (!JOB_ACTIVE && ReadBit(SPINDLE0SPEED))
{
SetBit (DRAWBAR_OPEN);
printf("Drawbar Open\n");
}
}
// Handle FeedHold/Resume
result = Debounce(ReadBit(FEEDHOLDBIT),&fcount,&flast,&flastsolid);
if (result == 1)
{
if (CS0_StoppingState == 0)
{
StopCoordinatedMotion();
printf("Feed Hold ON\n");
}
else
{
ResumeCoordinatedMotion();
printf("Feed Hold OFF\n");
}
}
// Handle Cycle Start
result = Debounce(ReadBit(CYCLESTARTBIT),&ccount,&clast,&clastsolid);
if (result == 1)
{
DoPC(PC_COMM_EXECUTE);
printf("Execute\n");
}
// Handle ESTOP pressed
result = Debounce(ReadBit(ESTOP),&ecount,&elast,&elastsolid);
if (result == 0)
{
printf("Emergency Stop ON\n");
DoPC(PC_COMM_ESTOP);
ClearBit (DRIVE_ENABLE);
ClearBit (SPINDLE_ENABLE);
Estop = 1; // Set a flag so that the release code only happens once
}
// Handle ESTOP released
result = Debounce(ReadBit(ESTOP),&ecount2,&elast2,&elastsolid2);
if (result == 1)
{
if (Estop == 1) // Check for flag
{
//DoPC(PC_COMM_IDLE);
printf("Emergency Stop OFF\n");
Estop = 0;
SetBit (DRIVE_ENABLE);
EnableAxis (0);
EnableAxis (1);
EnableAxis (2);
EnableAxis (4);
}
}
// Handle HALT
result = Debounce(ReadBit(HALTBIT),&hcount,&hlast,&hlastsolid);
if (result == 1)
{
DoPC(PC_COMM_HALT);
printf("Job Halt\n");
}
// Handle RESTART
result = Debounce(ReadBit(RESTARTBIT),&rcount,&rlast,&rlastsolid);
if (result == 1)
{
DoPC(PC_COMM_RESTART);
printf("Job Restart\n");
}
// Handle ZERO ALL
/*
result = Debounce(ReadBit(ZEROALLBIT),&zcount,&zlast,&zlastsolid);
if (result == 1)
{
DoPCFloat(PC_COMM_SET_X,0.0);
DoPCFloat(PC_COMM_SET_Y,0.0);
DoPCFloat(PC_COMM_SET_Z,0.0);
printf("Zero All Axes\n");
}
*/
}
// ************************************************************************
// 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);
}
// ************************************************************************
// 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;
}
// ************************************************************************
// Debounce a bit
//
// return 1 one time when first debounced high
// return 0 one time when first debounced low
// return -1 otherwise
#define DBTIME 300
int Debounce(int n, int *cnt, int *last, int *lastsolid)
{
int v = -1;
if (n == *last) // same as last time?
{
if (*cnt == DBTIME-1)
{
if (n != *lastsolid)
{
v = *lastsolid = n; // return debounced value
}
}
if (*cnt < DBTIME) (*cnt)++;
}
else
{
*cnt = 0; // reset count
}
*last = n;
return v;
}