I find if I configured the ch->LimitSwitchOptions=0x12f in th initializer thread , when the motor stop at end of the initializer c program, the "feed hold" status is activated, and then the g code (such as your example program named "DynoMotion.ngc") wouldn't run. But if I configured the LimitSwitchOptions=0x00 in the initial thread, the g code program can run. But it is dangerrous because there is no limit position detection. Why?
Your ch->LimitSwitchOptions if very strange and easily make the whole system stop and cann't rework.
The problem with limitation swich configuration
Moderators: TomKerekes, dynomotion
-
TomKerekes
- Posts: 2676
- Joined: Mon Dec 04, 2017 1:49 am
Re: The problem with limitation swich configuration
Hi yethsts,
You might be setting the wrong polarity for your Limit Switch Inputs. What state are the inputs when activated (touching limit)?
To use the opposite polarity configure: ch->LimitSwitchOptions=0x123
You might be setting the wrong polarity for your Limit Switch Inputs. What state are the inputs when activated (touching limit)?
To use the opposite polarity configure: ch->LimitSwitchOptions=0x123
Regards,
Tom Kerekes
Dynomotion, Inc.
Tom Kerekes
Dynomotion, Inc.
Re: The problem with limitation swich configuration
Yes, I am wrong. I configured the CH->LimitSwitchPosBit=0x00. Now I changed the 0x00 to the correct IO number, so the problem is solved.
But I have a new problem: In the end of initialization, I call the c function named zero() to reset the tool position (I guess). After it the number in axis window of kmotion be zero. And I can jog the all XYZ motors using the CNC's JOG button. But if I run the second line of your example gcode named "DynoMotion.ngc", the line is:G00 X-37.939 Y8.793 Z16.000 . It always report error: "soft limit Y+ rapid tranverse job halted". I have tried to change the Y8.793 to Y0 or Y-8.793, or change the ch->SoftLimitPos and ch->SoftLimitNeg to very big value such as: 1e+030 and -1e+030, but always the error happen. Why? (The first line of the DynoMotion.ngc is : G90 G21. I also selected/checked the "Machine Coord" and use the "abs coordinate" in CNC window.)
Sincerely,
Huang
But I have a new problem: In the end of initialization, I call the c function named zero() to reset the tool position (I guess). After it the number in axis window of kmotion be zero. And I can jog the all XYZ motors using the CNC's JOG button. But if I run the second line of your example gcode named "DynoMotion.ngc", the line is:G00 X-37.939 Y8.793 Z16.000 . It always report error: "soft limit Y+ rapid tranverse job halted". I have tried to change the Y8.793 to Y0 or Y-8.793, or change the ch->SoftLimitPos and ch->SoftLimitNeg to very big value such as: 1e+030 and -1e+030, but always the error happen. Why? (The first line of the DynoMotion.ngc is : G90 G21. I also selected/checked the "Machine Coord" and use the "abs coordinate" in CNC window.)
Sincerely,
Huang
Re: The problem with limitation swich configuration
And I also found if I change the first line code to G91 G21, then the next line of the gcode program can run.
-
TomKerekes
- Posts: 2676
- Joined: Mon Dec 04, 2017 1:49 am
Re: The problem with limitation swich configuration
Hi Huang,
What are your KMotionCNC | Tool Setup | Trajectory Planner | Axis Parameters | Cnts/inch ? A zero will cause a division by zero when machine counts to inches.
Otherwise please post your Initialization C Program and your KMotionCNC Tool Setup settings.
What are your KMotionCNC | Tool Setup | Trajectory Planner | Axis Parameters | Cnts/inch ? A zero will cause a division by zero when machine counts to inches.
Otherwise please post your Initialization C Program and your KMotionCNC Tool Setup settings.
Regards,
Tom Kerekes
Dynomotion, Inc.
Tom Kerekes
Dynomotion, Inc.
Re: The problem with limitation swich configuration
Sorry, there was a mistake in my yesterday's description. The fact is: if I use a very large number such as +-7e+6 for the SoftLimitPos and SoftLimitNeg, the g code can run. But if I use a smaller number such as +7e+5 for the SoftLimitPos and -7e+5 for the Neg, the system will output the error I yesterday said.
-
TomKerekes
- Posts: 2676
- Joined: Mon Dec 04, 2017 1:49 am
Re: The problem with limitation swich configuration
Please post your Initialization C Program and your KMotionCNC Tool Setup settings.
Regards,
Tom Kerekes
Dynomotion, Inc.
Tom Kerekes
Dynomotion, Inc.
This is the init program. I have commented the zero() part, but the error still appeared.
#define Lim0 183 // 我自己外加的测试用限位开关编号，接到kFlop板的通道0
#define Lim1 173 // 通道1接入的限位开关对应的IO编号
#define Lim2 172 // 通道2接入的限位开关对应的IO编号
#define Lim3 171 // 通道3接入的限位开关对应的IO编号
#define Lim4 168 // 通道4接入的限位开关对应的IO编号
#define Lim5 169 // 通道5接入的限位开关对应的IO编号
#define Lim6 170 // 通道6接入的限位开关对应的IO编号
// #define Lim7 // 目前未接，通道7接入的限位开关对应的IO编号
#define OutputCh0 8
#define OutputCh1 9
#define OutputCh2 10
#define OutputCh3 11
#define OutputCh4 12
#define OutputCh5 13
#define OutputCh6 14
#define OutputCh7 15
#define Relay1 29 // 控制通道0,1外接的闸
#define Relay2 30 // 控制通道2 外接的闸，控制x轴电机
#define Relay3 31 // 控制通道4,5外接的闸，控制y轴电机
#define Relay4 32 // 控制通道3,6外接的闸，控制z轴电机
#define StopWhenLow_Mode 0x10f
#define DisallowDriveIntoLimit_Mode 0x11f
#define StopMovement_Mode 0x12f
///////////////////////////////////////////////////////////////////////////////////////////////////
#define ESTOP 26 //红色停止按钮
#define Velocity 40000
#define SPEED 15000 // 移动速度
#define XAxis 2 // x轴对应的通道编号
#define YMasAxis 4 // y主轴对应的通道编号
#define YSlaAxis 5 // y从轴对应的通道编号
#define ZMasAxis 3 // z轴对应的通道编号
#define ZSlaAxis 6 // // z从轴对应的通道编号
#define Test_CH ch0
#define XAxis_CH ch2
#define YAxis_MASTER_CH ch4 //Y主轴通道名称
#define YAxis_SLAVE_CH ch5 //Y从轴通道名称
#define ZAxis_MASTER_CH ch3 //Z主轴通道名称
#define ZAxis_SLAVE_CH ch6 //Z从轴通道名称
#define X_Lim Lim2 // ch2
#define YMas_Lim Lim4 //ch4
#define YSla_Lim Lim5 //ch5
#define ZMas_Lim Lim3 //ch3
#define ZSla_Lim Lim6 //ch6
#define X_Relay Relay2 //ch2
#define Y_Relay Relay3 //ch4,ch5
#define Z_Relay Relay4 //ch3,ch6
#define X_OutputCh OutputCh2 //ch2
#define YMas_OutputCh OutputCh4 //ch4
#define YSla_OutputCh OutputCh5 //ch5
#define ZMas_OutputCh OutputCh3 //ch3
#define ZSla_OutputCh OutputCh6 //ch6
#define LimActOption StopMovement_Mode
/*
SetBit(UseRelay); // 抱闸，NPN三极管导通，继电器线圈得电，常闭触点断开，闸及限位开关供电0v，抱闸
ClearBit(UseRelay); // 松闸，NPN三极管截止，继电器线圈失点，常闭触点闭合，松闸，闸及限位开关供电24v
*/
main()
{
double T0, LastX = 0, LastY = 0, LastZ = 0, LastA = 0, Tau;
KStepPresent = TRUE; // enable KSTEP input multiplexing
FPGA(STEP_PULSE_LENGTH_ADD) = 63 + 0x80; // set polarity and pulse length to 4us
////////////////////////// X axis configuration //////////////////////////////////////////////
XAxis_CH->InputMode = NO_INPUT_MODE;
XAxis_CH->OutputMode = STEP_DIR_MODE;
XAxis_CH->Vel = Velocity;
XAxis_CH->Accel = 200000;
XAxis_CH->Jerk = 4e+006;
XAxis_CH->P = 0;
XAxis_CH->I = 0.01;
XAxis_CH->D = 0;
XAxis_CH->FFAccel = 0;
XAxis_CH->FFVel = 0;
XAxis_CH->MaxI = 200;
XAxis_CH->MaxErr = 1e+006;
XAxis_CH->MaxOutput = 200;
XAxis_CH->DeadBandGain = 1;
XAxis_CH->DeadBandRange = 0;
XAxis_CH->InputChan0 = 0;
XAxis_CH->InputChan1 = 0;
XAxis_CH->OutputChan0 = X_OutputCh; //
XAxis_CH->OutputChan1 = 0;
XAxis_CH->MasterAxis = -1;
//XAxis_CH->LimitSwitchOptions=0x0;
XAxis_CH->LimitSwitchOptions = 0x00; //
XAxis_CH->LimitSwitchNegBit = X_Lim;
XAxis_CH->LimitSwitchPosBit = X_Lim;
XAxis_CH->SoftLimitPos = 1e+009;
XAxis_CH->SoftLimitNeg = -1e+009;
XAxis_CH->InputGain0 = 1;
XAxis_CH->InputGain1 = 1;
XAxis_CH->InputOffset0 = 0;
XAxis_CH->InputOffset1 = 0;
XAxis_CH->OutputGain = 1;
XAxis_CH->OutputOffset = 0;
XAxis_CH->SlaveGain = 1;
XAxis_CH->BacklashMode = BACKLASH_OFF;
XAxis_CH->BacklashAmount = 0;
XAxis_CH->BacklashRate = 0;
XAxis_CH->invDistPerCycle = 1;
XAxis_CH->Lead = 0;
XAxis_CH->MaxFollowingError = 1000000000;
XAxis_CH->StepperAmplitude = 20;
XAxis_CH->iir[0].B0 = 1;
XAxis_CH->iir[0].B1 = 0;
XAxis_CH->iir[0].B2 = 0;
XAxis_CH->iir[0].A1 = 0;
XAxis_CH->iir[0].A2 = 0;
XAxis_CH->iir[1].B0 = 1;
XAxis_CH->iir[1].B1 = 0;
XAxis_CH->iir[1].B2 = 0;
XAxis_CH->iir[1].A1 = 0;
XAxis_CH->iir[1].A2 = 0;
XAxis_CH->iir[2].B0 = 0.000769;
XAxis_CH->iir[2].B1 = 0.001538;
XAxis_CH->iir[2].B2 = 0.000769;
XAxis_CH->iir[2].A1 = 1.92076;
XAxis_CH->iir[2].A2 = -0.923833;
///////////////////////////////////////// Y Axis Master configration ////////////////////////////////////////////
YAxis_MASTER_CH->InputMode = NO_INPUT_MODE;
YAxis_MASTER_CH->OutputMode = STEP_DIR_MODE;
YAxis_MASTER_CH->Vel = Velocity;
YAxis_MASTER_CH->Accel = 200000;
YAxis_MASTER_CH->Jerk = 4e+006;
YAxis_MASTER_CH->P = 0;
YAxis_MASTER_CH->I = 0.01;
YAxis_MASTER_CH->D = 0;
YAxis_MASTER_CH->FFAccel = 0;
YAxis_MASTER_CH->FFVel = 0;
YAxis_MASTER_CH->MaxI = 200;
YAxis_MASTER_CH->MaxErr = 1e+006;
YAxis_MASTER_CH->MaxOutput = 200;
YAxis_MASTER_CH->DeadBandGain = 1;
YAxis_MASTER_CH->DeadBandRange = 0;
YAxis_MASTER_CH->InputChan0 = 0;
YAxis_MASTER_CH->InputChan1 = 0;
YAxis_MASTER_CH->OutputChan0 = YMas_OutputCh; //
YAxis_MASTER_CH->OutputChan1 = 0;
YAxis_MASTER_CH->MasterAxis = -1;
//YAxis_MASTER_CH->LimitSwitchOptions=0x0;
YAxis_MASTER_CH->LimitSwitchOptions = 0x00; //115 105
YAxis_MASTER_CH->LimitSwitchNegBit = YMas_Lim;
YAxis_MASTER_CH->LimitSwitchPosBit = YMas_Lim;
YAxis_MASTER_CH->SoftLimitPos = 1e+009;
YAxis_MASTER_CH->SoftLimitNeg = -1e+009;
YAxis_MASTER_CH->InputGain0 = 1;
YAxis_MASTER_CH->InputGain1 = 1;
YAxis_MASTER_CH->InputOffset0 = 0;
YAxis_MASTER_CH->InputOffset1 = 0;
YAxis_MASTER_CH->OutputGain = 1;
YAxis_MASTER_CH->OutputOffset = 0;
YAxis_MASTER_CH->SlaveGain = 1;
YAxis_MASTER_CH->BacklashMode = BACKLASH_OFF;
YAxis_MASTER_CH->BacklashAmount = 0;
YAxis_MASTER_CH->BacklashRate = 0;
YAxis_MASTER_CH->invDistPerCycle = 1;
YAxis_MASTER_CH->Lead = 0;
YAxis_MASTER_CH->MaxFollowingError = 1000000000;
YAxis_MASTER_CH->StepperAmplitude = 20;
YAxis_MASTER_CH->iir[0].B0 = 1;
YAxis_MASTER_CH->iir[0].B1 = 0;
YAxis_MASTER_CH->iir[0].B2 = 0;
YAxis_MASTER_CH->iir[0].A1 = 0;
YAxis_MASTER_CH->iir[0].A2 = 0;
YAxis_MASTER_CH->iir[1].B0 = 1;
YAxis_MASTER_CH->iir[1].B1 = 0;
YAxis_MASTER_CH->iir[1].B2 = 0;
YAxis_MASTER_CH->iir[1].A1 = 0;
YAxis_MASTER_CH->iir[1].A2 = 0;
YAxis_MASTER_CH->iir[2].B0 = 0.000769;
YAxis_MASTER_CH->iir[2].B1 = 0.001538;
YAxis_MASTER_CH->iir[2].B2 = 0.000769;
YAxis_MASTER_CH->iir[2].A1 = 1.92076;
YAxis_MASTER_CH->iir[2].A2 = -0.923833;
////////////////////////////////////////// Y Axis Slave configration///////////////////////////////////////////////////////////
YAxis_SLAVE_CH->InputMode = NO_INPUT_MODE;
YAxis_SLAVE_CH->OutputMode = STEP_DIR_MODE;
YAxis_SLAVE_CH->Vel = Velocity;
YAxis_SLAVE_CH->Accel = 200000;
YAxis_SLAVE_CH->Jerk = 4e+006;
YAxis_SLAVE_CH->P = 0;
YAxis_SLAVE_CH->I = 0.01;
YAxis_SLAVE_CH->D = 0;
YAxis_SLAVE_CH->FFAccel = 0;
YAxis_SLAVE_CH->FFVel = 0;
YAxis_SLAVE_CH->MaxI = 200;
YAxis_SLAVE_CH->MaxErr = 1e+006;
YAxis_SLAVE_CH->MaxOutput = 200;
YAxis_SLAVE_CH->DeadBandGain = 1;
YAxis_SLAVE_CH->DeadBandRange = 0;
YAxis_SLAVE_CH->InputChan0 = 0;
YAxis_SLAVE_CH->InputChan1 = 0;
YAxis_SLAVE_CH->OutputChan0 = YSla_OutputCh; //
YAxis_SLAVE_CH->OutputChan1 = 0;
YAxis_SLAVE_CH->MasterAxis = YMasAxis;
//YAxis_SLAVE_CH->LimitSwitchOptions=0x0;
YAxis_SLAVE_CH->LimitSwitchOptions = 0x00; //115 105
YAxis_SLAVE_CH->LimitSwitchNegBit = YSla_Lim;
YAxis_SLAVE_CH->LimitSwitchPosBit = YSla_Lim;
YAxis_SLAVE_CH->SoftLimitPos = 1e+009;
YAxis_SLAVE_CH->SoftLimitNeg = -1e+009;
YAxis_SLAVE_CH->InputGain0 = 1;
YAxis_SLAVE_CH->InputGain1 = 1;
YAxis_SLAVE_CH->InputOffset0 = 0;
YAxis_SLAVE_CH->InputOffset1 = 0;
YAxis_SLAVE_CH->OutputGain = 1;
YAxis_SLAVE_CH->OutputOffset = 0;
YAxis_SLAVE_CH->SlaveGain = 1;
YAxis_SLAVE_CH->BacklashMode = BACKLASH_OFF;
YAxis_SLAVE_CH->BacklashAmount = 0;
YAxis_SLAVE_CH->BacklashRate = 0;
YAxis_SLAVE_CH->invDistPerCycle = 1;
YAxis_SLAVE_CH->Lead = 0;
YAxis_SLAVE_CH->MaxFollowingError = 1000000000;
YAxis_SLAVE_CH->StepperAmplitude = 20;
YAxis_SLAVE_CH->iir[0].B0 = 1;
YAxis_SLAVE_CH->iir[0].B1 = 0;
YAxis_SLAVE_CH->iir[0].B2 = 0;
YAxis_SLAVE_CH->iir[0].A1 = 0;
YAxis_SLAVE_CH->iir[0].A2 = 0;
YAxis_SLAVE_CH->iir[1].B0 = 1;
YAxis_SLAVE_CH->iir[1].B1 = 0;
YAxis_SLAVE_CH->iir[1].B2 = 0;
YAxis_SLAVE_CH->iir[1].A1 = 0;
YAxis_SLAVE_CH->iir[1].A2 = 0;
YAxis_SLAVE_CH->iir[2].B0 = 0.000769;
YAxis_SLAVE_CH->iir[2].B1 = 0.001538;
YAxis_SLAVE_CH->iir[2].B2 = 0.000769;
YAxis_SLAVE_CH->iir[2].A1 = 1.92076;
YAxis_SLAVE_CH->iir[2].A2 = -0.923833;
////////////////////////////////////////// Z Axis Master configration ////////////////////////////////////////////
ZAxis_MASTER_CH->InputMode = NO_INPUT_MODE;
ZAxis_MASTER_CH->OutputMode = STEP_DIR_MODE;
ZAxis_MASTER_CH->Vel = Velocity;
ZAxis_MASTER_CH->Accel = 200000;
ZAxis_MASTER_CH->Jerk = 4e+006;
ZAxis_MASTER_CH->P = 0;
ZAxis_MASTER_CH->I = 0.01;
ZAxis_MASTER_CH->D = 0;
ZAxis_MASTER_CH->FFAccel = 0;
ZAxis_MASTER_CH->FFVel = 0;
ZAxis_MASTER_CH->MaxI = 200;
ZAxis_MASTER_CH->MaxErr = 1e+006;
ZAxis_MASTER_CH->MaxOutput = 200;
ZAxis_MASTER_CH->DeadBandGain = 1;
ZAxis_MASTER_CH->DeadBandRange = 0;
ZAxis_MASTER_CH->InputChan0 = 0;
ZAxis_MASTER_CH->InputChan1 = 0;
ZAxis_MASTER_CH->OutputChan0 = ZMas_OutputCh; //
ZAxis_MASTER_CH->OutputChan1 = 0;
ZAxis_MASTER_CH->MasterAxis = -1;
//ZAxis_MASTER_CH->LimitSwitchOptions=0x0;
ZAxis_MASTER_CH->LimitSwitchOptions = 0x00; //115 105
ZAxis_MASTER_CH->LimitSwitchNegBit = ZMas_Lim;
ZAxis_MASTER_CH->LimitSwitchPosBit = ZMas_Lim;
ZAxis_MASTER_CH->SoftLimitPos = 1e+009;
ZAxis_MASTER_CH->SoftLimitNeg = -1e+009;
ZAxis_MASTER_CH->InputGain0 = 1;
ZAxis_MASTER_CH->InputGain1 = 1;
ZAxis_MASTER_CH->InputOffset0 = 0;
ZAxis_MASTER_CH->InputOffset1 = 0;
ZAxis_MASTER_CH->OutputGain = 1;
ZAxis_MASTER_CH->OutputOffset = 0;
ZAxis_MASTER_CH->SlaveGain = 1;
ZAxis_MASTER_CH->BacklashMode = BACKLASH_OFF;
ZAxis_MASTER_CH->BacklashAmount = 0;
ZAxis_MASTER_CH->BacklashRate = 0;
ZAxis_MASTER_CH->invDistPerCycle = 1;
ZAxis_MASTER_CH->Lead = 0;
ZAxis_MASTER_CH->MaxFollowingError = 1000000000;
ZAxis_MASTER_CH->StepperAmplitude = 20;
ZAxis_MASTER_CH->iir[0].B0 = 1;
ZAxis_MASTER_CH->iir[0].B1 = 0;
ZAxis_MASTER_CH->iir[0].B2 = 0;
ZAxis_MASTER_CH->iir[0].A1 = 0;
ZAxis_MASTER_CH->iir[0].A2 = 0;
ZAxis_MASTER_CH->iir[1].B0 = 1;
ZAxis_MASTER_CH->iir[1].B1 = 0;
ZAxis_MASTER_CH->iir[1].B2 = 0;
ZAxis_MASTER_CH->iir[1].A1 = 0;
ZAxis_MASTER_CH->iir[1].A2 = 0;
ZAxis_MASTER_CH->iir[2].B0 = 0.000769;
ZAxis_MASTER_CH->iir[2].B1 = 0.001538;
ZAxis_MASTER_CH->iir[2].B2 = 0.000769;
ZAxis_MASTER_CH->iir[2].A1 = 1.92076;
ZAxis_MASTER_CH->iir[2].A2 = -0.923833;
////////////////////////////////////////// Z Axis Slave connfigration/ //////////////////////////////////////////////////////////
ZAxis_SLAVE_CH->InputMode = NO_INPUT_MODE;
ZAxis_SLAVE_CH->OutputMode = STEP_DIR_MODE;
ZAxis_SLAVE_CH->Vel = Velocity;
ZAxis_SLAVE_CH->Accel = 200000;
ZAxis_SLAVE_CH->Jerk = 4e+006;
ZAxis_SLAVE_CH->P = 0;
ZAxis_SLAVE_CH->I = 0.01;
ZAxis_SLAVE_CH->D = 0;
ZAxis_SLAVE_CH->FFAccel = 0;
ZAxis_SLAVE_CH->FFVel = 0;
ZAxis_SLAVE_CH->MaxI = 200;
ZAxis_SLAVE_CH->MaxErr = 1e+006;
ZAxis_SLAVE_CH->MaxOutput = 200;
ZAxis_SLAVE_CH->DeadBandGain = 1;
ZAxis_SLAVE_CH->DeadBandRange = 0;
ZAxis_SLAVE_CH->InputChan0 = 0;
ZAxis_SLAVE_CH->InputChan1 = 0;
ZAxis_SLAVE_CH->OutputChan0 = ZSla_OutputCh; //
ZAxis_SLAVE_CH->OutputChan1 = 0;
ZAxis_SLAVE_CH->MasterAxis = ZMasAxis;
//ZAxis_SLAVE_CH->LimitSwitchOptions=0x0;
ZAxis_SLAVE_CH->LimitSwitchOptions = 0x00; //115 105
ZAxis_SLAVE_CH->LimitSwitchNegBit = ZSla_Lim;
ZAxis_SLAVE_CH->LimitSwitchPosBit = ZSla_Lim;
ZAxis_SLAVE_CH->SoftLimitPos = 1e+009;
ZAxis_SLAVE_CH->SoftLimitNeg = -1e+009;
ZAxis_SLAVE_CH->InputGain0 = 1;
ZAxis_SLAVE_CH->InputGain1 = 1;
ZAxis_SLAVE_CH->InputOffset0 = 0;
ZAxis_SLAVE_CH->InputOffset1 = 0;
ZAxis_SLAVE_CH->OutputGain = 1;
ZAxis_SLAVE_CH->OutputOffset = 0;
ZAxis_SLAVE_CH->SlaveGain = 1;
ZAxis_SLAVE_CH->BacklashMode = BACKLASH_OFF;
ZAxis_SLAVE_CH->BacklashAmount = 0;
ZAxis_SLAVE_CH->BacklashRate = 0;
ZAxis_SLAVE_CH->invDistPerCycle = 1;
ZAxis_SLAVE_CH->Lead = 0;
ZAxis_SLAVE_CH->MaxFollowingError = 1000000000;
ZAxis_SLAVE_CH->StepperAmplitude = 20;
ZAxis_SLAVE_CH->iir[0].B0 = 1;
ZAxis_SLAVE_CH->iir[0].B1 = 0;
ZAxis_SLAVE_CH->iir[0].B2 = 0;
ZAxis_SLAVE_CH->iir[0].A1 = 0;
ZAxis_SLAVE_CH->iir[0].A2 = 0;
ZAxis_SLAVE_CH->iir[1].B0 = 1;
ZAxis_SLAVE_CH->iir[1].B1 = 0;
ZAxis_SLAVE_CH->iir[1].B2 = 0;
ZAxis_SLAVE_CH->iir[1].A1 = 0;
ZAxis_SLAVE_CH->iir[1].A2 = 0;
ZAxis_SLAVE_CH->iir[2].B0 = 0.000769;
ZAxis_SLAVE_CH->iir[2].B1 = 0.001538;
ZAxis_SLAVE_CH->iir[2].B2 = 0.000769;
ZAxis_SLAVE_CH->iir[2].A1 = 1.92076;
ZAxis_SLAVE_CH->iir[2].A2 = -0.923833;
///////////////////////////////////////////////////////////////////////////////////////////////////////////
EnableAxisDest(XAxis, 0); // 使能X轴通道
EnableAxisDest(YMasAxis, 0); // 使能Y主通道
EnableAxisDest(YSlaAxis, 0); // 使能Y从轴通道
EnableAxisDest(ZMasAxis, 0); // 使能Z主通道
EnableAxisDest(ZSlaAxis, 0); // 使能Z从轴通道
// DisableAxis(0);
DefineCoordSystem(XAxis, YMasAxis, ZMasAxis, -1); // 设置x，y，z轴对应的通道
SetBitDirection(X_Relay, 1); // 设置Relay 1对应的端口为输出模式
ClearBit(X_Relay); // 置0，代表松闸
SetBitDirection(Y_Relay, 1); // 设置Relay 1对应的端口为输出模式
ClearBit(Y_Relay); // 置0，代表松闸
SetBitDirection(Z_Relay, 1); // 设置Relay 1对应的端口为输出模式
ClearBit(Z_Relay); // 置0，代表松闸
Delay_sec(2); //松闸需要时间
SetBitDirection(45, 1); // set the Enable pin of the KFlop to be output mode
SetBit(45); // Make the Enable signal of the kFlop to be 1
printf("3Axis Configuration Completed::\n");
/*
DisableAxis(XAxis); //
DisableAxis(YMasAxis); //
DisableAxis(YSlaAxis); //
DisableAxis(ZMasAxis); //
DisableAxis(ZSlaAxis); //
Zero(XAxis);
Zero(YMasAxis);
Zero(YSlaAxis);
Zero(ZMasAxis);
Zero(ZSlaAxis);
EnableAxis(XAxis); //
EnableAxis(YMasAxis); //
EnableAxis(YSlaAxis); //
EnableAxis(ZMasAxis); //
EnableAxis(ZSlaAxis); // */
XAxis_CH->LimitSwitchOptions = LimActOption;
YAxis_MASTER_CH->LimitSwitchOptions = LimActOption;
YAxis_SLAVE_CH->LimitSwitchOptions = LimActOption;
ZAxis_MASTER_CH->LimitSwitchOptions = LimActOption;
ZAxis_SLAVE_CH->LimitSwitchOptions = LimActOption;
Delay_sec(1);
// Assign soft limits to each channel
XAxis_CH->SoftLimitNeg = -700000.0;
XAxis_CH->SoftLimitPos = 700000.0;
YAxis_MASTER_CH->SoftLimitNeg = -300000;
YAxis_MASTER_CH->SoftLimitPos = 300000;
YAxis_SLAVE_CH->SoftLimitNeg = -300000;
YAxis_SLAVE_CH->SoftLimitPos = 300000;
ZAxis_MASTER_CH->SoftLimitNeg = -230000;
ZAxis_MASTER_CH->SoftLimitPos = 220000;
ZAxis_SLAVE_CH->SoftLimitNeg = -230000;
ZAxis_SLAVE_CH->SoftLimitPos = 220000;
#ifdef USEBRAKE //再松一次闸以防万一
ClearBit(Z_Relay);
Delay_sec(2); // 松闸需要时间
#endif
for (;;) // loop forever
{
WaitNextTimeSlice();
// Dsiable axes if Estop is pressed
if (!ReadBit(ESTOP))
{
DisableAxis(XAxis); // axis still enabled? - Disable it
DisableAxis(YMasAxis); // axis still enabled? - Disable it
DisableAxis(YSlaAxis); // axis still enabled? - Disable it
DisableAxis(ZMasAxis); // axis still enabled? - Disable it
DisableAxis(ZSlaAxis); // axis still enabled? - Disable it
break;
}
// Service Amplifier disable after no activity for a while
if (XAxis_CH->Dest != LastX || YAxis_MASTER_CH->Dest != LastY || ZAxis_MASTER_CH->Dest != LastZ)
{
// we moved - enable KStep Amplifers
SetBit(45);
T0 = Time_sec(); // record the time and position of last motion
LastX = XAxis_CH->Dest;
LastY = YAxis_MASTER_CH->Dest;
LastZ = ZAxis_MASTER_CH->Dest;
}
else
{
if (Time_sec() > T0 + 60.0) ClearBit(45);
}
}
}
#define Lim1 173 // 通道1接入的限位开关对应的IO编号
#define Lim2 172 // 通道2接入的限位开关对应的IO编号
#define Lim3 171 // 通道3接入的限位开关对应的IO编号
#define Lim4 168 // 通道4接入的限位开关对应的IO编号
#define Lim5 169 // 通道5接入的限位开关对应的IO编号
#define Lim6 170 // 通道6接入的限位开关对应的IO编号
// #define Lim7 // 目前未接，通道7接入的限位开关对应的IO编号
#define OutputCh0 8
#define OutputCh1 9
#define OutputCh2 10
#define OutputCh3 11
#define OutputCh4 12
#define OutputCh5 13
#define OutputCh6 14
#define OutputCh7 15
#define Relay1 29 // 控制通道0,1外接的闸
#define Relay2 30 // 控制通道2 外接的闸，控制x轴电机
#define Relay3 31 // 控制通道4,5外接的闸，控制y轴电机
#define Relay4 32 // 控制通道3,6外接的闸，控制z轴电机
#define StopWhenLow_Mode 0x10f
#define DisallowDriveIntoLimit_Mode 0x11f
#define StopMovement_Mode 0x12f
///////////////////////////////////////////////////////////////////////////////////////////////////
#define ESTOP 26 //红色停止按钮
#define Velocity 40000
#define SPEED 15000 // 移动速度
#define XAxis 2 // x轴对应的通道编号
#define YMasAxis 4 // y主轴对应的通道编号
#define YSlaAxis 5 // y从轴对应的通道编号
#define ZMasAxis 3 // z轴对应的通道编号
#define ZSlaAxis 6 // // z从轴对应的通道编号
#define Test_CH ch0
#define XAxis_CH ch2
#define YAxis_MASTER_CH ch4 //Y主轴通道名称
#define YAxis_SLAVE_CH ch5 //Y从轴通道名称
#define ZAxis_MASTER_CH ch3 //Z主轴通道名称
#define ZAxis_SLAVE_CH ch6 //Z从轴通道名称
#define X_Lim Lim2 // ch2
#define YMas_Lim Lim4 //ch4
#define YSla_Lim Lim5 //ch5
#define ZMas_Lim Lim3 //ch3
#define ZSla_Lim Lim6 //ch6
#define X_Relay Relay2 //ch2
#define Y_Relay Relay3 //ch4,ch5
#define Z_Relay Relay4 //ch3,ch6
#define X_OutputCh OutputCh2 //ch2
#define YMas_OutputCh OutputCh4 //ch4
#define YSla_OutputCh OutputCh5 //ch5
#define ZMas_OutputCh OutputCh3 //ch3
#define ZSla_OutputCh OutputCh6 //ch6
#define LimActOption StopMovement_Mode
/*
SetBit(UseRelay); // 抱闸，NPN三极管导通，继电器线圈得电，常闭触点断开，闸及限位开关供电0v，抱闸
ClearBit(UseRelay); // 松闸，NPN三极管截止，继电器线圈失点，常闭触点闭合，松闸，闸及限位开关供电24v
*/
main()
{
double T0, LastX = 0, LastY = 0, LastZ = 0, LastA = 0, Tau;
KStepPresent = TRUE; // enable KSTEP input multiplexing
FPGA(STEP_PULSE_LENGTH_ADD) = 63 + 0x80; // set polarity and pulse length to 4us
////////////////////////// X axis configuration //////////////////////////////////////////////
XAxis_CH->InputMode = NO_INPUT_MODE;
XAxis_CH->OutputMode = STEP_DIR_MODE;
XAxis_CH->Vel = Velocity;
XAxis_CH->Accel = 200000;
XAxis_CH->Jerk = 4e+006;
XAxis_CH->P = 0;
XAxis_CH->I = 0.01;
XAxis_CH->D = 0;
XAxis_CH->FFAccel = 0;
XAxis_CH->FFVel = 0;
XAxis_CH->MaxI = 200;
XAxis_CH->MaxErr = 1e+006;
XAxis_CH->MaxOutput = 200;
XAxis_CH->DeadBandGain = 1;
XAxis_CH->DeadBandRange = 0;
XAxis_CH->InputChan0 = 0;
XAxis_CH->InputChan1 = 0;
XAxis_CH->OutputChan0 = X_OutputCh; //
XAxis_CH->OutputChan1 = 0;
XAxis_CH->MasterAxis = -1;
//XAxis_CH->LimitSwitchOptions=0x0;
XAxis_CH->LimitSwitchOptions = 0x00; //
XAxis_CH->LimitSwitchNegBit = X_Lim;
XAxis_CH->LimitSwitchPosBit = X_Lim;
XAxis_CH->SoftLimitPos = 1e+009;
XAxis_CH->SoftLimitNeg = -1e+009;
XAxis_CH->InputGain0 = 1;
XAxis_CH->InputGain1 = 1;
XAxis_CH->InputOffset0 = 0;
XAxis_CH->InputOffset1 = 0;
XAxis_CH->OutputGain = 1;
XAxis_CH->OutputOffset = 0;
XAxis_CH->SlaveGain = 1;
XAxis_CH->BacklashMode = BACKLASH_OFF;
XAxis_CH->BacklashAmount = 0;
XAxis_CH->BacklashRate = 0;
XAxis_CH->invDistPerCycle = 1;
XAxis_CH->Lead = 0;
XAxis_CH->MaxFollowingError = 1000000000;
XAxis_CH->StepperAmplitude = 20;
XAxis_CH->iir[0].B0 = 1;
XAxis_CH->iir[0].B1 = 0;
XAxis_CH->iir[0].B2 = 0;
XAxis_CH->iir[0].A1 = 0;
XAxis_CH->iir[0].A2 = 0;
XAxis_CH->iir[1].B0 = 1;
XAxis_CH->iir[1].B1 = 0;
XAxis_CH->iir[1].B2 = 0;
XAxis_CH->iir[1].A1 = 0;
XAxis_CH->iir[1].A2 = 0;
XAxis_CH->iir[2].B0 = 0.000769;
XAxis_CH->iir[2].B1 = 0.001538;
XAxis_CH->iir[2].B2 = 0.000769;
XAxis_CH->iir[2].A1 = 1.92076;
XAxis_CH->iir[2].A2 = -0.923833;
///////////////////////////////////////// Y Axis Master configration ////////////////////////////////////////////
YAxis_MASTER_CH->InputMode = NO_INPUT_MODE;
YAxis_MASTER_CH->OutputMode = STEP_DIR_MODE;
YAxis_MASTER_CH->Vel = Velocity;
YAxis_MASTER_CH->Accel = 200000;
YAxis_MASTER_CH->Jerk = 4e+006;
YAxis_MASTER_CH->P = 0;
YAxis_MASTER_CH->I = 0.01;
YAxis_MASTER_CH->D = 0;
YAxis_MASTER_CH->FFAccel = 0;
YAxis_MASTER_CH->FFVel = 0;
YAxis_MASTER_CH->MaxI = 200;
YAxis_MASTER_CH->MaxErr = 1e+006;
YAxis_MASTER_CH->MaxOutput = 200;
YAxis_MASTER_CH->DeadBandGain = 1;
YAxis_MASTER_CH->DeadBandRange = 0;
YAxis_MASTER_CH->InputChan0 = 0;
YAxis_MASTER_CH->InputChan1 = 0;
YAxis_MASTER_CH->OutputChan0 = YMas_OutputCh; //
YAxis_MASTER_CH->OutputChan1 = 0;
YAxis_MASTER_CH->MasterAxis = -1;
//YAxis_MASTER_CH->LimitSwitchOptions=0x0;
YAxis_MASTER_CH->LimitSwitchOptions = 0x00; //115 105
YAxis_MASTER_CH->LimitSwitchNegBit = YMas_Lim;
YAxis_MASTER_CH->LimitSwitchPosBit = YMas_Lim;
YAxis_MASTER_CH->SoftLimitPos = 1e+009;
YAxis_MASTER_CH->SoftLimitNeg = -1e+009;
YAxis_MASTER_CH->InputGain0 = 1;
YAxis_MASTER_CH->InputGain1 = 1;
YAxis_MASTER_CH->InputOffset0 = 0;
YAxis_MASTER_CH->InputOffset1 = 0;
YAxis_MASTER_CH->OutputGain = 1;
YAxis_MASTER_CH->OutputOffset = 0;
YAxis_MASTER_CH->SlaveGain = 1;
YAxis_MASTER_CH->BacklashMode = BACKLASH_OFF;
YAxis_MASTER_CH->BacklashAmount = 0;
YAxis_MASTER_CH->BacklashRate = 0;
YAxis_MASTER_CH->invDistPerCycle = 1;
YAxis_MASTER_CH->Lead = 0;
YAxis_MASTER_CH->MaxFollowingError = 1000000000;
YAxis_MASTER_CH->StepperAmplitude = 20;
YAxis_MASTER_CH->iir[0].B0 = 1;
YAxis_MASTER_CH->iir[0].B1 = 0;
YAxis_MASTER_CH->iir[0].B2 = 0;
YAxis_MASTER_CH->iir[0].A1 = 0;
YAxis_MASTER_CH->iir[0].A2 = 0;
YAxis_MASTER_CH->iir[1].B0 = 1;
YAxis_MASTER_CH->iir[1].B1 = 0;
YAxis_MASTER_CH->iir[1].B2 = 0;
YAxis_MASTER_CH->iir[1].A1 = 0;
YAxis_MASTER_CH->iir[1].A2 = 0;
YAxis_MASTER_CH->iir[2].B0 = 0.000769;
YAxis_MASTER_CH->iir[2].B1 = 0.001538;
YAxis_MASTER_CH->iir[2].B2 = 0.000769;
YAxis_MASTER_CH->iir[2].A1 = 1.92076;
YAxis_MASTER_CH->iir[2].A2 = -0.923833;
////////////////////////////////////////// Y Axis Slave configration///////////////////////////////////////////////////////////
YAxis_SLAVE_CH->InputMode = NO_INPUT_MODE;
YAxis_SLAVE_CH->OutputMode = STEP_DIR_MODE;
YAxis_SLAVE_CH->Vel = Velocity;
YAxis_SLAVE_CH->Accel = 200000;
YAxis_SLAVE_CH->Jerk = 4e+006;
YAxis_SLAVE_CH->P = 0;
YAxis_SLAVE_CH->I = 0.01;
YAxis_SLAVE_CH->D = 0;
YAxis_SLAVE_CH->FFAccel = 0;
YAxis_SLAVE_CH->FFVel = 0;
YAxis_SLAVE_CH->MaxI = 200;
YAxis_SLAVE_CH->MaxErr = 1e+006;
YAxis_SLAVE_CH->MaxOutput = 200;
YAxis_SLAVE_CH->DeadBandGain = 1;
YAxis_SLAVE_CH->DeadBandRange = 0;
YAxis_SLAVE_CH->InputChan0 = 0;
YAxis_SLAVE_CH->InputChan1 = 0;
YAxis_SLAVE_CH->OutputChan0 = YSla_OutputCh; //
YAxis_SLAVE_CH->OutputChan1 = 0;
YAxis_SLAVE_CH->MasterAxis = YMasAxis;
//YAxis_SLAVE_CH->LimitSwitchOptions=0x0;
YAxis_SLAVE_CH->LimitSwitchOptions = 0x00; //115 105
YAxis_SLAVE_CH->LimitSwitchNegBit = YSla_Lim;
YAxis_SLAVE_CH->LimitSwitchPosBit = YSla_Lim;
YAxis_SLAVE_CH->SoftLimitPos = 1e+009;
YAxis_SLAVE_CH->SoftLimitNeg = -1e+009;
YAxis_SLAVE_CH->InputGain0 = 1;
YAxis_SLAVE_CH->InputGain1 = 1;
YAxis_SLAVE_CH->InputOffset0 = 0;
YAxis_SLAVE_CH->InputOffset1 = 0;
YAxis_SLAVE_CH->OutputGain = 1;
YAxis_SLAVE_CH->OutputOffset = 0;
YAxis_SLAVE_CH->SlaveGain = 1;
YAxis_SLAVE_CH->BacklashMode = BACKLASH_OFF;
YAxis_SLAVE_CH->BacklashAmount = 0;
YAxis_SLAVE_CH->BacklashRate = 0;
YAxis_SLAVE_CH->invDistPerCycle = 1;
YAxis_SLAVE_CH->Lead = 0;
YAxis_SLAVE_CH->MaxFollowingError = 1000000000;
YAxis_SLAVE_CH->StepperAmplitude = 20;
YAxis_SLAVE_CH->iir[0].B0 = 1;
YAxis_SLAVE_CH->iir[0].B1 = 0;
YAxis_SLAVE_CH->iir[0].B2 = 0;
YAxis_SLAVE_CH->iir[0].A1 = 0;
YAxis_SLAVE_CH->iir[0].A2 = 0;
YAxis_SLAVE_CH->iir[1].B0 = 1;
YAxis_SLAVE_CH->iir[1].B1 = 0;
YAxis_SLAVE_CH->iir[1].B2 = 0;
YAxis_SLAVE_CH->iir[1].A1 = 0;
YAxis_SLAVE_CH->iir[1].A2 = 0;
YAxis_SLAVE_CH->iir[2].B0 = 0.000769;
YAxis_SLAVE_CH->iir[2].B1 = 0.001538;
YAxis_SLAVE_CH->iir[2].B2 = 0.000769;
YAxis_SLAVE_CH->iir[2].A1 = 1.92076;
YAxis_SLAVE_CH->iir[2].A2 = -0.923833;
////////////////////////////////////////// Z Axis Master configration ////////////////////////////////////////////
ZAxis_MASTER_CH->InputMode = NO_INPUT_MODE;
ZAxis_MASTER_CH->OutputMode = STEP_DIR_MODE;
ZAxis_MASTER_CH->Vel = Velocity;
ZAxis_MASTER_CH->Accel = 200000;
ZAxis_MASTER_CH->Jerk = 4e+006;
ZAxis_MASTER_CH->P = 0;
ZAxis_MASTER_CH->I = 0.01;
ZAxis_MASTER_CH->D = 0;
ZAxis_MASTER_CH->FFAccel = 0;
ZAxis_MASTER_CH->FFVel = 0;
ZAxis_MASTER_CH->MaxI = 200;
ZAxis_MASTER_CH->MaxErr = 1e+006;
ZAxis_MASTER_CH->MaxOutput = 200;
ZAxis_MASTER_CH->DeadBandGain = 1;
ZAxis_MASTER_CH->DeadBandRange = 0;
ZAxis_MASTER_CH->InputChan0 = 0;
ZAxis_MASTER_CH->InputChan1 = 0;
ZAxis_MASTER_CH->OutputChan0 = ZMas_OutputCh; //
ZAxis_MASTER_CH->OutputChan1 = 0;
ZAxis_MASTER_CH->MasterAxis = -1;
//ZAxis_MASTER_CH->LimitSwitchOptions=0x0;
ZAxis_MASTER_CH->LimitSwitchOptions = 0x00; //115 105
ZAxis_MASTER_CH->LimitSwitchNegBit = ZMas_Lim;
ZAxis_MASTER_CH->LimitSwitchPosBit = ZMas_Lim;
ZAxis_MASTER_CH->SoftLimitPos = 1e+009;
ZAxis_MASTER_CH->SoftLimitNeg = -1e+009;
ZAxis_MASTER_CH->InputGain0 = 1;
ZAxis_MASTER_CH->InputGain1 = 1;
ZAxis_MASTER_CH->InputOffset0 = 0;
ZAxis_MASTER_CH->InputOffset1 = 0;
ZAxis_MASTER_CH->OutputGain = 1;
ZAxis_MASTER_CH->OutputOffset = 0;
ZAxis_MASTER_CH->SlaveGain = 1;
ZAxis_MASTER_CH->BacklashMode = BACKLASH_OFF;
ZAxis_MASTER_CH->BacklashAmount = 0;
ZAxis_MASTER_CH->BacklashRate = 0;
ZAxis_MASTER_CH->invDistPerCycle = 1;
ZAxis_MASTER_CH->Lead = 0;
ZAxis_MASTER_CH->MaxFollowingError = 1000000000;
ZAxis_MASTER_CH->StepperAmplitude = 20;
ZAxis_MASTER_CH->iir[0].B0 = 1;
ZAxis_MASTER_CH->iir[0].B1 = 0;
ZAxis_MASTER_CH->iir[0].B2 = 0;
ZAxis_MASTER_CH->iir[0].A1 = 0;
ZAxis_MASTER_CH->iir[0].A2 = 0;
ZAxis_MASTER_CH->iir[1].B0 = 1;
ZAxis_MASTER_CH->iir[1].B1 = 0;
ZAxis_MASTER_CH->iir[1].B2 = 0;
ZAxis_MASTER_CH->iir[1].A1 = 0;
ZAxis_MASTER_CH->iir[1].A2 = 0;
ZAxis_MASTER_CH->iir[2].B0 = 0.000769;
ZAxis_MASTER_CH->iir[2].B1 = 0.001538;
ZAxis_MASTER_CH->iir[2].B2 = 0.000769;
ZAxis_MASTER_CH->iir[2].A1 = 1.92076;
ZAxis_MASTER_CH->iir[2].A2 = -0.923833;
////////////////////////////////////////// Z Axis Slave connfigration/ //////////////////////////////////////////////////////////
ZAxis_SLAVE_CH->InputMode = NO_INPUT_MODE;
ZAxis_SLAVE_CH->OutputMode = STEP_DIR_MODE;
ZAxis_SLAVE_CH->Vel = Velocity;
ZAxis_SLAVE_CH->Accel = 200000;
ZAxis_SLAVE_CH->Jerk = 4e+006;
ZAxis_SLAVE_CH->P = 0;
ZAxis_SLAVE_CH->I = 0.01;
ZAxis_SLAVE_CH->D = 0;
ZAxis_SLAVE_CH->FFAccel = 0;
ZAxis_SLAVE_CH->FFVel = 0;
ZAxis_SLAVE_CH->MaxI = 200;
ZAxis_SLAVE_CH->MaxErr = 1e+006;
ZAxis_SLAVE_CH->MaxOutput = 200;
ZAxis_SLAVE_CH->DeadBandGain = 1;
ZAxis_SLAVE_CH->DeadBandRange = 0;
ZAxis_SLAVE_CH->InputChan0 = 0;
ZAxis_SLAVE_CH->InputChan1 = 0;
ZAxis_SLAVE_CH->OutputChan0 = ZSla_OutputCh; //
ZAxis_SLAVE_CH->OutputChan1 = 0;
ZAxis_SLAVE_CH->MasterAxis = ZMasAxis;
//ZAxis_SLAVE_CH->LimitSwitchOptions=0x0;
ZAxis_SLAVE_CH->LimitSwitchOptions = 0x00; //115 105
ZAxis_SLAVE_CH->LimitSwitchNegBit = ZSla_Lim;
ZAxis_SLAVE_CH->LimitSwitchPosBit = ZSla_Lim;
ZAxis_SLAVE_CH->SoftLimitPos = 1e+009;
ZAxis_SLAVE_CH->SoftLimitNeg = -1e+009;
ZAxis_SLAVE_CH->InputGain0 = 1;
ZAxis_SLAVE_CH->InputGain1 = 1;
ZAxis_SLAVE_CH->InputOffset0 = 0;
ZAxis_SLAVE_CH->InputOffset1 = 0;
ZAxis_SLAVE_CH->OutputGain = 1;
ZAxis_SLAVE_CH->OutputOffset = 0;
ZAxis_SLAVE_CH->SlaveGain = 1;
ZAxis_SLAVE_CH->BacklashMode = BACKLASH_OFF;
ZAxis_SLAVE_CH->BacklashAmount = 0;
ZAxis_SLAVE_CH->BacklashRate = 0;
ZAxis_SLAVE_CH->invDistPerCycle = 1;
ZAxis_SLAVE_CH->Lead = 0;
ZAxis_SLAVE_CH->MaxFollowingError = 1000000000;
ZAxis_SLAVE_CH->StepperAmplitude = 20;
ZAxis_SLAVE_CH->iir[0].B0 = 1;
ZAxis_SLAVE_CH->iir[0].B1 = 0;
ZAxis_SLAVE_CH->iir[0].B2 = 0;
ZAxis_SLAVE_CH->iir[0].A1 = 0;
ZAxis_SLAVE_CH->iir[0].A2 = 0;
ZAxis_SLAVE_CH->iir[1].B0 = 1;
ZAxis_SLAVE_CH->iir[1].B1 = 0;
ZAxis_SLAVE_CH->iir[1].B2 = 0;
ZAxis_SLAVE_CH->iir[1].A1 = 0;
ZAxis_SLAVE_CH->iir[1].A2 = 0;
ZAxis_SLAVE_CH->iir[2].B0 = 0.000769;
ZAxis_SLAVE_CH->iir[2].B1 = 0.001538;
ZAxis_SLAVE_CH->iir[2].B2 = 0.000769;
ZAxis_SLAVE_CH->iir[2].A1 = 1.92076;
ZAxis_SLAVE_CH->iir[2].A2 = -0.923833;
///////////////////////////////////////////////////////////////////////////////////////////////////////////
EnableAxisDest(XAxis, 0); // 使能X轴通道
EnableAxisDest(YMasAxis, 0); // 使能Y主通道
EnableAxisDest(YSlaAxis, 0); // 使能Y从轴通道
EnableAxisDest(ZMasAxis, 0); // 使能Z主通道
EnableAxisDest(ZSlaAxis, 0); // 使能Z从轴通道
// DisableAxis(0);
DefineCoordSystem(XAxis, YMasAxis, ZMasAxis, -1); // 设置x，y，z轴对应的通道
SetBitDirection(X_Relay, 1); // 设置Relay 1对应的端口为输出模式
ClearBit(X_Relay); // 置0，代表松闸
SetBitDirection(Y_Relay, 1); // 设置Relay 1对应的端口为输出模式
ClearBit(Y_Relay); // 置0，代表松闸
SetBitDirection(Z_Relay, 1); // 设置Relay 1对应的端口为输出模式
ClearBit(Z_Relay); // 置0，代表松闸
Delay_sec(2); //松闸需要时间
SetBitDirection(45, 1); // set the Enable pin of the KFlop to be output mode
SetBit(45); // Make the Enable signal of the kFlop to be 1
printf("3Axis Configuration Completed::\n");
/*
DisableAxis(XAxis); //
DisableAxis(YMasAxis); //
DisableAxis(YSlaAxis); //
DisableAxis(ZMasAxis); //
DisableAxis(ZSlaAxis); //
Zero(XAxis);
Zero(YMasAxis);
Zero(YSlaAxis);
Zero(ZMasAxis);
Zero(ZSlaAxis);
EnableAxis(XAxis); //
EnableAxis(YMasAxis); //
EnableAxis(YSlaAxis); //
EnableAxis(ZMasAxis); //
EnableAxis(ZSlaAxis); // */
XAxis_CH->LimitSwitchOptions = LimActOption;
YAxis_MASTER_CH->LimitSwitchOptions = LimActOption;
YAxis_SLAVE_CH->LimitSwitchOptions = LimActOption;
ZAxis_MASTER_CH->LimitSwitchOptions = LimActOption;
ZAxis_SLAVE_CH->LimitSwitchOptions = LimActOption;
Delay_sec(1);
// Assign soft limits to each channel
XAxis_CH->SoftLimitNeg = -700000.0;
XAxis_CH->SoftLimitPos = 700000.0;
YAxis_MASTER_CH->SoftLimitNeg = -300000;
YAxis_MASTER_CH->SoftLimitPos = 300000;
YAxis_SLAVE_CH->SoftLimitNeg = -300000;
YAxis_SLAVE_CH->SoftLimitPos = 300000;
ZAxis_MASTER_CH->SoftLimitNeg = -230000;
ZAxis_MASTER_CH->SoftLimitPos = 220000;
ZAxis_SLAVE_CH->SoftLimitNeg = -230000;
ZAxis_SLAVE_CH->SoftLimitPos = 220000;
#ifdef USEBRAKE //再松一次闸以防万一
ClearBit(Z_Relay);
Delay_sec(2); // 松闸需要时间
#endif
for (;;) // loop forever
{
WaitNextTimeSlice();
// Dsiable axes if Estop is pressed
if (!ReadBit(ESTOP))
{
DisableAxis(XAxis); // axis still enabled? - Disable it
DisableAxis(YMasAxis); // axis still enabled? - Disable it
DisableAxis(YSlaAxis); // axis still enabled? - Disable it
DisableAxis(ZMasAxis); // axis still enabled? - Disable it
DisableAxis(ZSlaAxis); // axis still enabled? - Disable it
break;
}
// Service Amplifier disable after no activity for a while
if (XAxis_CH->Dest != LastX || YAxis_MASTER_CH->Dest != LastY || ZAxis_MASTER_CH->Dest != LastZ)
{
// we moved - enable KStep Amplifers
SetBit(45);
T0 = Time_sec(); // record the time and position of last motion
LastX = XAxis_CH->Dest;
LastY = YAxis_MASTER_CH->Dest;
LastZ = ZAxis_MASTER_CH->Dest;
}
else
{
if (Time_sec() > T0 + 60.0) ClearBit(45);
}
}
}
Attachment is the Tools setting. I have changed the Cnts/Inch value of all x,y,z axis from 40000 to 20000. Things are slightly better, for example the XAxis_CH->SoftLimitPos must be larger than 1100000 before when Cnts/inci is 40000, but now needs only > 700000 for ok. But inner essence is the same.
- Attachments
-
Re: The problem with limitation swich configuration
After initialization, I can use CNC's Jog button to drive the 5 motors moving. But can't run the example g code named DynoMotion.ngc. Awalys report: : "soft limit Y+ rapid tranverse job halted" or ": "soft limit X+ rapid tranverse job halted"."