DRO Not Zero After Homing
Moderators: TomKerekes, dynomotion
DRO Not Zero After Homing
Hi there,
Please see my initialization file attached.
I can't seem to get KMotionCNC's DRO to show zero on XYZ after my homing routine is complete.
Am I using the Zero command incorrectly to clear the measured position?
Thank you in advance,
Cyprian
Please see my initialization file attached.
I can't seem to get KMotionCNC's DRO to show zero on XYZ after my homing routine is complete.
Am I using the Zero command incorrectly to clear the measured position?
Thank you in advance,
Cyprian
- Attachments
-
- SimpleHome3AxisProjectCNC17.c
- (19.87 KiB) Downloaded 113 times
- TomKerekes
- Posts: 2635
- Joined: Mon Dec 04, 2017 1:49 am
Re: DRO Not Zero After Homing
Hi Cyprian,
Do you have any GCode Offsets? Are the Machine Coordinates or KMotion.exe Axis Screen's Destinations zero? Does your machine have encoders? Are you displaying encoders?
Do you have any GCode Offsets? Are the Machine Coordinates or KMotion.exe Axis Screen's Destinations zero? Does your machine have encoders? Are you displaying encoders?
Regards,
Tom Kerekes
Dynomotion, Inc.
Tom Kerekes
Dynomotion, Inc.
Re: DRO Not Zero After Homing
Thank you kindly Tom,
Yes I did indeed have a G54 work offset.
When I removed the offset the DRO reads zero when homed.
Yes I did indeed have a G54 work offset.
When I removed the offset the DRO reads zero when homed.
Re: DRO Not Zero After Homing
hi;
I have the same issue , but my problem is as follows
I have a machine that when I home it the machine coordinates "Red DRO display"in X,Y , instead of showing 0000, it shows 25.4..and I cant change it or reset it.
now I have set up a controller running another kflop/ Kanalog board , but with same version of software, and c programs including initialisation and the homing program in the home computer, but on this machine the machine home position "Red DRO display " sets to zero automatically when the home switch is hit.
can you please give me an idea of what can I be doing wrong?
I running ver 4.35H in both machines .
Kind regards
Mariano.
I have the same issue , but my problem is as follows
I have a machine that when I home it the machine coordinates "Red DRO display"in X,Y , instead of showing 0000, it shows 25.4..and I cant change it or reset it.
now I have set up a controller running another kflop/ Kanalog board , but with same version of software, and c programs including initialisation and the homing program in the home computer, but on this machine the machine home position "Red DRO display " sets to zero automatically when the home switch is hit.
can you please give me an idea of what can I be doing wrong?
I running ver 4.35H in both machines .
Kind regards
Mariano.
- TomKerekes
- Posts: 2635
- Joined: Mon Dec 04, 2017 1:49 am
Re: DRO Not Zero After Homing
Hi Mariano,
Please post your Home and Initialization Programs.
Please post your Home and Initialization Programs.
Regards,
Tom Kerekes
Dynomotion, Inc.
Tom Kerekes
Dynomotion, Inc.
Re: DRO Not Zero After Homing
hi Tom;
sorry for the delay , but i have been trying a couple of things like flashing the Kflop and doing a recovery , and the controller still shows the 25.4000 in X and Y DRO machine position, in fact when I start the Kmotion CNC program it reads 0.0000 in X and Y , with the Kanalog not initialised , and the DRO numbers in yellow ,but within a few seconds of showing that yellow 0,0000 in the X and Y DRO it changes to X25.4000 Y25.4000, in yellow, and with the controller still in pre initialisation stage , and without running the limit stops program .
kind Regards
Mariano
sorry for the delay , but i have been trying a couple of things like flashing the Kflop and doing a recovery , and the controller still shows the 25.4000 in X and Y DRO machine position, in fact when I start the Kmotion CNC program it reads 0.0000 in X and Y , with the Kanalog not initialised , and the DRO numbers in yellow ,but within a few seconds of showing that yellow 0,0000 in the X and Y DRO it changes to X25.4000 Y25.4000, in yellow, and with the controller still in pre initialisation stage , and without running the limit stops program .
kind Regards
Mariano
Re: DRO Not Zero After Homing
Code: Select all
[code]
#include "KMotionDef.h"
#define TMP 10 // which spare persist to use to transfer data
#include "KflopToKMotionCNCFunctions.c"
// Example Init program that includes "smooth" MPG motion example
// which makes use of the exponential motion command.
#define QA 27 // define to which IO bits the AB signals are connected
#define QB 26
#define ESTOP_BIT 16//shorting to pin 3, 3.3v to pin 5 bit 16
#define TAU 0.08 // smoothness factor (Low Pass Time constant seconds)
#define FINAL_TIME 1.0 // Set final dest after this amount of time with no change
#define SELECTX 28
#define SELECTY 29
#define SELECTZ 30
#define SELECTA 31
#define SELECTB 32
#define SELECTC 33
#define FACTOR1 21
#define FACTOR10 34
#define FACTOR100 35
// variables calculo visualizador velocidad hilo en pantalla
void ServiceTacoRpm(void);
#define V_HILO 9.166 // velocidad de salida del hilo en mm por revolucion del motor
#define VOLT 3 // voltaje a velocidad maxima del tacometro
#define CYCLE_TIME 0.3 //seconds between velocity estimations
#define VEL 170 // usa esta variable en el DROlabel para mostrar la velocidad de giro
char TS[80];
// variables y definicion de funciones control comunicacion pc-kflop
void ServiceWatchdogStatus(void);
void WatchdogTripped(void);
void WatchdogOK(void);
#define WATCHDOG_DELAY 5.0 // seconds after host stops to Trigger
main()
{
int BitA,Change1=0,Change2=0, DiffX2;
int PosNoWrap, NewPos, Pos=0, wraps;
int InMotion=FALSE,Axis,LastAxis=-1;
double LastChangeTime=0,Target,Factor=0;
FPGA(STEP_PULSE_LENGTH_ADD) = 32 + 0x40; // set the pulse time to ~ 2µs and multiplex to JP4 and JP6
//FPGA(STEP_PULSE_LENGTH_ADD) = 63 + 0x80; // set polarity and pulse length to 4us
ch0->InputMode=NO_INPUT_MODE;
ch0->OutputMode=STEP_DIR_MODE;
ch0->Vel=3000;
ch0->Accel=60000;
ch0->Jerk=4e+006;
ch0->P=0;
ch0->I=0.01;
ch0->D=0;
ch0->FFAccel=0;
ch0->FFVel=0;
ch0->MaxI=200;
ch0->MaxErr=1e+006;
ch0->MaxOutput=200;
ch0->DeadBandGain=1;
ch0->DeadBandRange=0;
ch0->InputChan0=0;
ch0->InputChan1=0;
ch0->OutputChan0=8;
ch0->OutputChan1=0;
ch0->MasterAxis=-1;
ch0->LimitSwitchOptions=0x0;
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=1000000000;
ch0->StepperAmplitude=20;
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.000769;
ch0->iir[2].B1=0.001538;
ch0->iir[2].B2=0.000769;
ch0->iir[2].A1=1.92076;
ch0->iir[2].A2=-0.923833;
EnableAxisDest(0,ch0->Dest);
ch1->InputMode=NO_INPUT_MODE;
ch1->OutputMode=STEP_DIR_MODE;
ch1->Vel=4000;
ch1->Accel=2000;
ch1->Jerk=4e+006;
ch1->P=0;
ch1->I=0.01;
ch1->D=0;
ch1->FFAccel=0;
ch1->FFVel=0;
ch1->MaxI=200;
ch1->MaxErr=1e+006;
ch1->MaxOutput=200;
ch1->DeadBandGain=1;
ch1->DeadBandRange=0;
ch1->InputChan0=0;
ch1->InputChan1=0;
ch1->OutputChan0=9;
ch1->OutputChan1=0;
ch1->MasterAxis=-1;
ch1->LimitSwitchOptions=0x0;
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=1000000000;
ch1->StepperAmplitude=20;
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.000769;
ch1->iir[2].B1=0.001538;
ch1->iir[2].B2=0.000769;
ch1->iir[2].A1=1.92076;
ch1->iir[2].A2=-0.923833;
EnableAxisDest(1,ch1->Dest);
ch4->InputMode=NO_INPUT_MODE;
ch4->OutputMode=STEP_DIR_MODE;
ch4->Vel=4000;
ch4->Accel=28000;
ch4->Jerk=4e+006;
ch4->P=0;
ch4->I=0.01;
ch4->D=0;
ch4->FFAccel=0;
ch4->FFVel=0;
ch4->MaxI=200;
ch4->MaxErr=1e+006;
ch4->MaxOutput=200;
ch4->DeadBandGain=1;
ch4->DeadBandRange=0;
ch4->InputChan0=4;
ch4->InputChan1=0;
ch4->OutputChan0=12;
ch4->OutputChan1=0;
ch4->MasterAxis=-1;
ch4->LimitSwitchOptions=0x0;
ch4->SoftLimitPos=1e+008;
ch4->SoftLimitNeg=-1e+008;
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=1000000000;
ch4->StepperAmplitude=20;
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=0.000769;
ch4->iir[2].B1=0.001538;
ch4->iir[2].B2=0.000769;
ch4->iir[2].A1=1.92076;
ch4->iir[2].A2=-0.923833;
EnableAxisDest(4,ch4->Dest);
ch5->InputMode=NO_INPUT_MODE;
ch5->OutputMode=STEP_DIR_MODE;
ch5->Vel=4000;
ch5->Accel=40000;
ch5->Jerk=4e+006;
ch5->P=0;
ch5->I=0.01;
ch5->D=0;
ch5->FFAccel=0;
ch5->FFVel=0;
ch5->MaxI=200;
ch5->MaxErr=1e+006;
ch5->MaxOutput=200;
ch5->DeadBandGain=1;
ch5->DeadBandRange=0;
ch5->InputChan0=4;
ch5->InputChan1=0;
ch5->OutputChan0=13;
ch5->OutputChan1=0;
ch5->MasterAxis=-1;
ch5->LimitSwitchOptions=0x0;
ch5->SoftLimitPos=1e+009;
ch5->SoftLimitNeg=-1e+009;
ch5->InputGain0=-1;
ch5->InputGain1=1;
ch5->InputOffset0=0;
ch5->InputOffset1=0;
ch5->OutputGain=-1;
ch5->OutputOffset=0;
ch5->SlaveGain=1;
ch5->BacklashMode=BACKLASH_LINEAR;
ch5->BacklashAmount=55.62;
ch5->BacklashRate=500;
ch5->invDistPerCycle=1;
ch5->Lead=0;
ch5->MaxFollowingError=1000000000;
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=0.000769;
ch5->iir[2].B1=0.001538;
ch5->iir[2].B2=0.000769;
ch5->iir[2].A1=1.92076;
ch5->iir[2].A2=-0.923833;
EnableAxisDest(5,ch5->Dest);
ch6->InputMode=NO_INPUT_MODE;
ch6->OutputMode=STEP_DIR_MODE;
ch6->Vel=4000;
ch6->Accel=40000;
ch6->Jerk=4e+006;
ch6->P=0;
ch6->I=0.01;
ch6->D=0;
ch6->FFAccel=0;
ch6->FFVel=0;
ch6->MaxI=200;
ch6->MaxErr=1e+006;
ch6->MaxOutput=200;
ch6->DeadBandGain=1;
ch6->DeadBandRange=0;
ch6->InputChan0=6;
ch6->InputChan1=0;
ch6->OutputChan0=14;
ch6->OutputChan1=0;
ch6->MasterAxis=-1;
ch6->LimitSwitchOptions=0x0;
ch6->SoftLimitPos=1e+009;
ch6->SoftLimitNeg=-1e+009;
ch6->InputGain0=-1;
ch6->InputGain1=1;
ch6->InputOffset0=0;
ch6->InputOffset1=0;
ch6->OutputGain=1;
ch6->OutputOffset=0;
ch6->SlaveGain=1;
ch6->BacklashMode=BACKLASH_OFF;
ch6->BacklashAmount=0;
ch6->BacklashRate=0;
ch6->invDistPerCycle=1;
ch6->Lead=0;
ch6->MaxFollowingError=1000000000;
ch6->StepperAmplitude=20;
ch6->iir[0].B0=1;
ch6->iir[0].B1=0;
ch6->iir[0].B2=0;
ch6->iir[0].A1=0;
ch6->iir[0].A2=0;
ch6->iir[1].B0=1;
ch6->iir[1].B1=0;
ch6->iir[1].B2=0;
ch6->iir[1].A1=0;
ch6->iir[1].A2=0;
ch6->iir[2].B0=0.000769;
ch6->iir[2].B1=0.001538;
ch6->iir[2].B2=0.000769;
ch6->iir[2].A1=1.92076;
ch6->iir[2].A2=-0.923833;
EnableAxisDest(6,ch6->Dest);
ch7->InputMode=NO_INPUT_MODE;
ch7->OutputMode=STEP_DIR_MODE;
ch7->Vel=4000;
ch7->Accel=40000;
ch7->Jerk=4e+006;
ch7->P=0;
ch7->I=0.01;
ch7->D=0;
ch7->FFAccel=0;
ch7->FFVel=0;
ch7->MaxI=200;
ch7->MaxErr=1e+006;
ch7->MaxOutput=200;
ch7->DeadBandGain=1;
ch7->DeadBandRange=0;
ch7->InputChan0=6;
ch7->InputChan1=0;
ch7->OutputChan0=15;
ch7->OutputChan1=0;
ch7->MasterAxis=-1;
ch7->LimitSwitchOptions=0x0;
ch7->SoftLimitPos=1e+009;
ch7->SoftLimitNeg=-1e+009;
ch7->InputGain0=-1;
ch7->InputGain1=1;
ch7->InputOffset0=0;
ch7->InputOffset1=0;
ch7->OutputGain=-1;
ch7->OutputOffset=0;
ch7->SlaveGain=1;
ch7->BacklashMode=BACKLASH_OFF;
ch7->BacklashAmount=0;
ch7->BacklashRate=0;
ch7->invDistPerCycle=1;
ch7->Lead=0;
ch7->MaxFollowingError=1000000000;
ch7->StepperAmplitude=20;
ch7->iir[0].B0=1;
ch7->iir[0].B1=0;
ch7->iir[0].B2=0;
ch7->iir[0].A1=0;
ch7->iir[0].A2=0;
ch7->iir[1].B0=1;
ch7->iir[1].B1=0;
ch7->iir[1].B2=0;
ch7->iir[1].A1=0;
ch7->iir[1].A2=0;
ch7->iir[2].B0=0.000769;
ch7->iir[2].B1=0.001538;
ch7->iir[2].B2=0.000769;
ch7->iir[2].A1=1.92076;
ch7->iir[2].A2=-0.923833;
EnableAxisDest(7,ch7->Dest);
DefineCoordSystem6(4,5,0,6,7,1);
for (;;)
{
// convert quadrature to 2 bit binary
BitA = ReadBit(QA);
PosNoWrap = (ReadBit(QB) ^ BitA) | (BitA<<1);
// Diff between expected position based on average of two prev deltas
// and position with no wraps. (Keep as X2 to avoid division by 2)
// DiffX2 = 2*(Pos-PosNoWrap) + (Change2+Change1);
DiffX2 = 2*(Pos-PosNoWrap);
// Calc quadrature wraparounds to bring Diff nearest zero
// offset by 128 wraps to avoid requiring floor()
wraps = ((DiffX2+1028)>>3)-128;
// factor in the quadrature wraparounds
NewPos = PosNoWrap + (wraps<<2);
Change2 = Change1;
Change1 = NewPos - Pos;
Pos = NewPos;
if (ReadBit(FACTOR1)) // is X1 selected?
Factor = 0.04;
else if (ReadBit(FACTOR10)) // is X10 selected?
Factor = 0.4;
else if (ReadBit(FACTOR100)) // is X100 selected?
Factor = 4.0;
else
Factor = 0.0;
if (ReadBit(SELECTX)) // is x selected?
Axis=4;
else if (ReadBit(SELECTY)) // is y selected?
Axis=5;
else if (ReadBit(SELECTZ)) // is z selected?
Axis=0;
else if (ReadBit(SELECTA)) // is A selected?
Axis=6;
else if (ReadBit(SELECTB)) // is y selected?
Axis=7;
else if (ReadBit(SELECTC)) // is y selected?
Axis=1;
else
Axis=-1;
// check if the Axis just changed or we have been
// converging to the target for a long time
if (Axis != LastAxis ||
(InMotion && Time_sec() > LastChangeTime+FINAL_TIME))
{
if (InMotion)
Move(LastAxis,Target); //finalize any motion
LastAxis = Axis;
InMotion = FALSE;
}
if (Change1) // did we move?
{
if (!InMotion) Target = chan[Axis].Dest;
Target += Change1 * Factor;
MoveExp(Axis,Target,TAU); // note: contains a WaitNextTimeSlice
LastChangeTime = Time_sec();
InMotion=TRUE;
}
else
{
WaitNextTimeSlice();
}
if (ReadBit(16)) // estop??
{
DisableAxis(0);
DisableAxis(1);
DisableAxis(4);
DisableAxis(5);
DisableAxis(6);
DisableAxis(7);
ClearBit(160);
ClearBit(144);
ClearBit(145);
ClearBit(146);
ClearBit(147);
}
WaitNextTimeSlice();
ServiceTacoRpm();
ServiceWatchdogStatus();
}
return 0;
}
// FUNCION MUESTRA AVANCE HILO EN PANTALLA
void ServiceTacoRpm(void)
{
static double T0=0.0; // remember the last time we turned on
double Q=Time_sec(); // get current Time_sec
if (T0==0.0 || Q > T0 + CYCLE_TIME)
{
double P1 = KANALOG_CONVERT_ADC_TO_VOLTS(ADC(4)); // Read current position
sprintf(TS,"%.1f dm/min\n", P1 * (100.99*VOLT/V_HILO)); // Display RPM
DROLabel(200, VEL, TS); // mandamos los datos a la pantalla
T0=Q; // set start time for the next cycle
}
}
// FUNCIONES QUE CONTROLAN LA COMUNICACION PC-KFLOP
void ServiceWatchdogStatus(void)
{
static int Alive=FALSE;
static int PrevStatusRequestCounter=-1;
static double WatchdogTime=0;
double T=Time_sec();
// check if Host is requesting Status
if (StatusRequestCounter != PrevStatusRequestCounter)
{
// yes, save time
WatchdogTime = T + WATCHDOG_DELAY;
PrevStatusRequestCounter=StatusRequestCounter;
if (!Alive) WatchdogOK();
Alive=TRUE;
}
else
{
if (T > WatchdogTime) // time to trigger?
{
if (Alive) WatchdogTripped();
Alive=FALSE;
}
}
}
void WatchdogOK(void)
{
/*
ClearBit(47); // KFLOP LED off
Delay_sec(1);
SetBit(47);
Delay_sec(1);
*/
}
void WatchdogTripped(void)
{
DisableAxis(0);
DisableAxis(1);
DisableAxis(4);
DisableAxis(5);
DisableAxis(6);
DisableAxis(7);
ClearBit(160);
ClearBit(144);
ClearBit(145);
ClearBit(146);
ClearBit(147);
}
#include "KMotionDef.h"
#include "SimpleHomeIndexFunction.c"
#define TMP 10
#include "KflopToKMotionCNCFunctions.c"
//#include "KFLOPtoPCCmdExamples.c"
// home subroutine
main()
{
int result;
ch4->LimitSwitchOptions=0x0;
ch4->SoftLimitPos=1e30;
ch4->SoftLimitNeg=-1e30;
ch5->LimitSwitchOptions=0x0;
ch5->SoftLimitPos=1e30;
ch5->SoftLimitNeg=-1e30;
// HOME X
result = SimpleHomeIndexFunction(4, // axis number to home
2000.0, // speed to move toward home
-1, // direction to move toward home (+1 or -1)
137, // limit bit number to watch for
0, // limit polarity to wait for (1 or 0)
100.0, // speed to move while searching for index
-1, // index bit number to watch for (use -1 for none)
0, // index polarity to wait for (1 or 0)
200); // amount to move inside limits
EnableAxisDest(4,0.0); // cero maquina en X
//MDI("G49");
//DoPCFloat(PC_COMM_SET_X,0.0);
if (result==0)
{
printf("Home X succeded\n");
}
else
{
printf("Home X failed\n");
return;
}
// HOME Y
result = SimpleHomeIndexFunction(5, // axis number to home
5000.0, // speed to move toward home
-1, // direction to move toward home (+1 or -1)
136, // limit bit number to watch for
0, // limit polarity to wait for (1 or 0)
100.0, // speed to move while searching for index
-1, // index bit number to watch for (use -1 for none)
1, // index polarity to wait for (1 or 0)
200); // amount to move inside limits
EnableAxisDest(5,0.0); // cero maquina en Y
if (result==0)
{
printf("Home Y succeded\n");
}
else
{
printf("Home Y failed\n");
return;
}
ch4->LimitSwitchOptions=0x0;
ch4->SoftLimitPos=500165;
ch4->SoftLimitNeg=-500;
ch5->LimitSwitchOptions=0x0;
ch5->SoftLimitPos=578500;
ch5->SoftLimitNeg=-500;
DoPC(PC_COMM_HALT); // stop KMotionCNC
}
Re: DRO Not Zero After Homing
this is the testing rig
on this one the X and Y DRO reads as it should zero in machine position (red numbers)
#include "KMotionDef.h"
#define TMP 10 // which spare persist to use to transfer data
#include "KflopToKMotionCNCFunctions.c"
// Example Init program that includes "smooth" MPG motion example
// which makes use of the exponential motion command.
#define QA 27 // define to which IO bits the AB signals are connected
#define QB 26
#define ESTOP_BIT 16//shorting to pin 3, 3.3v to pin 5 bit 16
#define TAU 0.08 // smoothness factor (Low Pass Time constant seconds)
#define FINAL_TIME 1.0 // Set final dest after this amount of time with no change
#define SELECTX 28
#define SELECTY 29
#define SELECTZ 30
#define SELECTA 31
#define SELECTB 32
#define SELECTC 33
#define FACTOR1 21
#define FACTOR10 34
#define FACTOR100 35
// variables calculo visualizador velocidad hilo en pantalla
void ServiceTacoRpm(void);
#define V_HILO 9.166 // velocidad de salida del hilo en mm por revolucion del motor
#define VOLT 3 // voltaje a velocidad maxima del tacometro
#define CYCLE_TIME 0.3 //seconds between velocity estimations
#define VEL 170 // usa esta variable en el DROlabel para mostrar la velocidad de giro
char TS[80];
// variables y definicion de funciones control comunicacion pc-kflop
void ServiceWatchdogStatus(void);
void WatchdogTripped(void);
void WatchdogOK(void);
#define WATCHDOG_DELAY 5.0 // seconds after host stops to Trigger
main()
{
int BitA,Change1=0,Change2=0, DiffX2;
int PosNoWrap, NewPos, Pos=0, wraps;
int InMotion=FALSE,Axis,LastAxis=-1;
double LastChangeTime=0,Target,Factor=0;
FPGA(STEP_PULSE_LENGTH_ADD) = 32 + 0x40; // set the pulse time to ~ 2µs and multiplex to JP4 and JP6
//FPGA(STEP_PULSE_LENGTH_ADD) = 63 + 0x80; // set polarity and pulse length to 4us
ch0->InputMode=NO_INPUT_MODE;
ch0->OutputMode=STEP_DIR_MODE;
ch0->Vel=3000;
ch0->Accel=60000;
ch0->Jerk=4e+006;
ch0->P=0;
ch0->I=0.01;
ch0->D=0;
ch0->FFAccel=0;
ch0->FFVel=0;
ch0->MaxI=200;
ch0->MaxErr=1e+006;
ch0->MaxOutput=200;
ch0->DeadBandGain=1;
ch0->DeadBandRange=0;
ch0->InputChan0=0;
ch0->InputChan1=0;
ch0->OutputChan0=8;
ch0->OutputChan1=0;
ch0->MasterAxis=-1;
ch0->LimitSwitchOptions=0x0;
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=1000000000;
ch0->StepperAmplitude=20;
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.000769;
ch0->iir[2].B1=0.001538;
ch0->iir[2].B2=0.000769;
ch0->iir[2].A1=1.92076;
ch0->iir[2].A2=-0.923833;
EnableAxisDest(0,ch0->Dest);
ch1->InputMode=NO_INPUT_MODE;
ch1->OutputMode=STEP_DIR_MODE;
ch1->Vel=4000;
ch1->Accel=2000;
ch1->Jerk=4e+006;
ch1->P=0;
ch1->I=0.01;
ch1->D=0;
ch1->FFAccel=0;
ch1->FFVel=0;
ch1->MaxI=200;
ch1->MaxErr=1e+006;
ch1->MaxOutput=200;
ch1->DeadBandGain=1;
ch1->DeadBandRange=0;
ch1->InputChan0=0;
ch1->InputChan1=0;
ch1->OutputChan0=9;
ch1->OutputChan1=0;
ch1->MasterAxis=-1;
ch1->LimitSwitchOptions=0x0;
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=1000000000;
ch1->StepperAmplitude=20;
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.000769;
ch1->iir[2].B1=0.001538;
ch1->iir[2].B2=0.000769;
ch1->iir[2].A1=1.92076;
ch1->iir[2].A2=-0.923833;
EnableAxisDest(1,ch1->Dest);
ch4->InputMode=NO_INPUT_MODE;
ch4->OutputMode=STEP_DIR_MODE;
ch4->Vel=4000;
ch4->Accel=28000;
ch4->Jerk=4e+006;
ch4->P=0;
ch4->I=0.01;
ch4->D=0;
ch4->FFAccel=0;
ch4->FFVel=0;
ch4->MaxI=200;
ch4->MaxErr=1e+006;
ch4->MaxOutput=200;
ch4->DeadBandGain=1;
ch4->DeadBandRange=0;
ch4->InputChan0=4;
ch4->InputChan1=0;
ch4->OutputChan0=12;
ch4->OutputChan1=0;
ch4->MasterAxis=-1;
ch4->LimitSwitchOptions=0x0;
ch4->SoftLimitPos=1e+008;
ch4->SoftLimitNeg=-1e+008;
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=1000000000;
ch4->StepperAmplitude=20;
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=0.000769;
ch4->iir[2].B1=0.001538;
ch4->iir[2].B2=0.000769;
ch4->iir[2].A1=1.92076;
ch4->iir[2].A2=-0.923833;
EnableAxisDest(4,ch4->Dest);
ch5->InputMode=NO_INPUT_MODE;
ch5->OutputMode=STEP_DIR_MODE;
ch5->Vel=4000;
ch5->Accel=40000;
ch5->Jerk=4e+006;
ch5->P=0;
ch5->I=0.01;
ch5->D=0;
ch5->FFAccel=0;
ch5->FFVel=0;
ch5->MaxI=200;
ch5->MaxErr=1e+006;
ch5->MaxOutput=200;
ch5->DeadBandGain=1;
ch5->DeadBandRange=0;
ch5->InputChan0=4;
ch5->InputChan1=0;
ch5->OutputChan0=13;
ch5->OutputChan1=0;
ch5->MasterAxis=-1;
ch5->LimitSwitchOptions=0x0;
ch5->SoftLimitPos=1e+009;
ch5->SoftLimitNeg=-1e+009;
ch5->InputGain0=-1;
ch5->InputGain1=1;
ch5->InputOffset0=0;
ch5->InputOffset1=0;
ch5->OutputGain=-1;
ch5->OutputOffset=0;
ch5->SlaveGain=1;
ch5->BacklashMode=BACKLASH_LINEAR;
ch5->BacklashAmount=55.62;
ch5->BacklashRate=500;
ch5->invDistPerCycle=1;
ch5->Lead=0;
ch5->MaxFollowingError=1000000000;
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=0.000769;
ch5->iir[2].B1=0.001538;
ch5->iir[2].B2=0.000769;
ch5->iir[2].A1=1.92076;
ch5->iir[2].A2=-0.923833;
EnableAxisDest(5,ch5->Dest);
ch6->InputMode=NO_INPUT_MODE;
ch6->OutputMode=STEP_DIR_MODE;
ch6->Vel=4000;
ch6->Accel=40000;
ch6->Jerk=4e+006;
ch6->P=0;
ch6->I=0.01;
ch6->D=0;
ch6->FFAccel=0;
ch6->FFVel=0;
ch6->MaxI=200;
ch6->MaxErr=1e+006;
ch6->MaxOutput=200;
ch6->DeadBandGain=1;
ch6->DeadBandRange=0;
ch6->InputChan0=6;
ch6->InputChan1=0;
ch6->OutputChan0=14;
ch6->OutputChan1=0;
ch6->MasterAxis=-1;
ch6->LimitSwitchOptions=0x0;
ch6->SoftLimitPos=1e+009;
ch6->SoftLimitNeg=-1e+009;
ch6->InputGain0=-1;
ch6->InputGain1=1;
ch6->InputOffset0=0;
ch6->InputOffset1=0;
ch6->OutputGain=1;
ch6->OutputOffset=0;
ch6->SlaveGain=1;
ch6->BacklashMode=BACKLASH_OFF;
ch6->BacklashAmount=0;
ch6->BacklashRate=0;
ch6->invDistPerCycle=1;
ch6->Lead=0;
ch6->MaxFollowingError=1000000000;
ch6->StepperAmplitude=20;
ch6->iir[0].B0=1;
ch6->iir[0].B1=0;
ch6->iir[0].B2=0;
ch6->iir[0].A1=0;
ch6->iir[0].A2=0;
ch6->iir[1].B0=1;
ch6->iir[1].B1=0;
ch6->iir[1].B2=0;
ch6->iir[1].A1=0;
ch6->iir[1].A2=0;
ch6->iir[2].B0=0.000769;
ch6->iir[2].B1=0.001538;
ch6->iir[2].B2=0.000769;
ch6->iir[2].A1=1.92076;
ch6->iir[2].A2=-0.923833;
EnableAxisDest(6,ch6->Dest);
ch7->InputMode=NO_INPUT_MODE;
ch7->OutputMode=STEP_DIR_MODE;
ch7->Vel=4000;
ch7->Accel=40000;
ch7->Jerk=4e+006;
ch7->P=0;
ch7->I=0.01;
ch7->D=0;
ch7->FFAccel=0;
ch7->FFVel=0;
ch7->MaxI=200;
ch7->MaxErr=1e+006;
ch7->MaxOutput=200;
ch7->DeadBandGain=1;
ch7->DeadBandRange=0;
ch7->InputChan0=6;
ch7->InputChan1=0;
ch7->OutputChan0=15;
ch7->OutputChan1=0;
ch7->MasterAxis=-1;
ch7->LimitSwitchOptions=0x0;
ch7->SoftLimitPos=1e+009;
ch7->SoftLimitNeg=-1e+009;
ch7->InputGain0=-1;
ch7->InputGain1=1;
ch7->InputOffset0=0;
ch7->InputOffset1=0;
ch7->OutputGain=-1;
ch7->OutputOffset=0;
ch7->SlaveGain=1;
ch7->BacklashMode=BACKLASH_OFF;
ch7->BacklashAmount=0;
ch7->BacklashRate=0;
ch7->invDistPerCycle=1;
ch7->Lead=0;
ch7->MaxFollowingError=1000000000;
ch7->StepperAmplitude=20;
ch7->iir[0].B0=1;
ch7->iir[0].B1=0;
ch7->iir[0].B2=0;
ch7->iir[0].A1=0;
ch7->iir[0].A2=0;
ch7->iir[1].B0=1;
ch7->iir[1].B1=0;
ch7->iir[1].B2=0;
ch7->iir[1].A1=0;
ch7->iir[1].A2=0;
ch7->iir[2].B0=0.000769;
ch7->iir[2].B1=0.001538;
ch7->iir[2].B2=0.000769;
ch7->iir[2].A1=1.92076;
ch7->iir[2].A2=-0.923833;
EnableAxisDest(7,ch7->Dest);
DefineCoordSystem6(4,5,0,6,7,1);
for (;;)
{
// convert quadrature to 2 bit binary
BitA = ReadBit(QA);
PosNoWrap = (ReadBit(QB) ^ BitA) | (BitA<<1);
// Diff between expected position based on average of two prev deltas
// and position with no wraps. (Keep as X2 to avoid division by 2)
// DiffX2 = 2*(Pos-PosNoWrap) + (Change2+Change1);
DiffX2 = 2*(Pos-PosNoWrap);
// Calc quadrature wraparounds to bring Diff nearest zero
// offset by 128 wraps to avoid requiring floor()
wraps = ((DiffX2+1028)>>3)-128;
// factor in the quadrature wraparounds
NewPos = PosNoWrap + (wraps<<2);
Change2 = Change1;
Change1 = NewPos - Pos;
Pos = NewPos;
if (ReadBit(FACTOR1)) // is X1 selected?
Factor = 0.04;
else if (ReadBit(FACTOR10)) // is X10 selected?
Factor = 0.4;
else if (ReadBit(FACTOR100)) // is X100 selected?
Factor = 4.0;
else
Factor = 0.0;
if (ReadBit(SELECTX)) // is x selected?
Axis=4;
else if (ReadBit(SELECTY)) // is y selected?
Axis=5;
else if (ReadBit(SELECTZ)) // is z selected?
Axis=0;
else if (ReadBit(SELECTA)) // is A selected?
Axis=6;
else if (ReadBit(SELECTB)) // is y selected?
Axis=7;
else if (ReadBit(SELECTC)) // is y selected?
Axis=1;
else
Axis=-1;
// check if the Axis just changed or we have been
// converging to the target for a long time
if (Axis != LastAxis ||
(InMotion && Time_sec() > LastChangeTime+FINAL_TIME))
{
if (InMotion)
Move(LastAxis,Target); //finalize any motion
LastAxis = Axis;
InMotion = FALSE;
}
if (Change1) // did we move?
{
if (!InMotion) Target = chan[Axis].Dest;
Target += Change1 * Factor;
MoveExp(Axis,Target,TAU); // note: contains a WaitNextTimeSlice
LastChangeTime = Time_sec();
InMotion=TRUE;
}
else
{
WaitNextTimeSlice();
}
if (ReadBit(16)) // estop??
{
DisableAxis(0);
DisableAxis(1);
DisableAxis(4);
DisableAxis(5);
DisableAxis(6);
DisableAxis(7);
ClearBit(160);
ClearBit(144);
ClearBit(145);
ClearBit(146);
ClearBit(147);
}
WaitNextTimeSlice();
ServiceTacoRpm();
ServiceWatchdogStatus();
}
return 0;
}
// FUNCION MUESTRA AVANCE HILO EN PANTALLA
void ServiceTacoRpm(void)
{
static double T0=0.0; // remember the last time we turned on
double Q=Time_sec(); // get current Time_sec
if (T0==0.0 || Q > T0 + CYCLE_TIME)
{
double P1 = KANALOG_CONVERT_ADC_TO_VOLTS(ADC(4)); // Read current position
sprintf(TS,"%.1f dm/min\n", P1 * (100.99*VOLT/V_HILO)); // Display RPM
DROLabel(200, VEL, TS); // mandamos los datos a la pantalla
T0=Q; // set start time for the next cycle
}
}
// FUNCIONES QUE CONTROLAN LA COMUNICACION PC-KFLOP
void ServiceWatchdogStatus(void)
{
static int Alive=FALSE;
static int PrevStatusRequestCounter=-1;
static double WatchdogTime=0;
double T=Time_sec();
// check if Host is requesting Status
if (StatusRequestCounter != PrevStatusRequestCounter)
{
// yes, save time
WatchdogTime = T + WATCHDOG_DELAY;
PrevStatusRequestCounter=StatusRequestCounter;
if (!Alive) WatchdogOK();
Alive=TRUE;
}
else
{
if (T > WatchdogTime) // time to trigger?
{
if (Alive) WatchdogTripped();
Alive=FALSE;
}
}
}
void WatchdogOK(void)
{
/*
ClearBit(47); // KFLOP LED off
Delay_sec(1);
SetBit(47);
Delay_sec(1);
*/
}
void WatchdogTripped(void)
{
DisableAxis(0);
DisableAxis(1);
DisableAxis(4);
DisableAxis(5);
DisableAxis(6);
DisableAxis(7);
ClearBit(160);
ClearBit(144);
ClearBit(145);
ClearBit(146);
ClearBit(147);
}
#include "KMotionDef.h"
#include "SimpleHomeIndexFunction.c"
#define TMP 10
#include "KflopToKMotionCNCFunctions.c"
//#include "KFLOPtoPCCmdExamples.c"
// home subroutine
main()
{
int result;
ch4->LimitSwitchOptions=0x0;
ch4->SoftLimitPos=1e30;
ch4->SoftLimitNeg=-1e30;
ch5->LimitSwitchOptions=0x0;
ch5->SoftLimitPos=1e30;
ch5->SoftLimitNeg=-1e30;
// HOME X
result = SimpleHomeIndexFunction(4, // axis number to home
2000.0, // speed to move toward home
-1, // direction to move toward home (+1 or -1)
137, // limit bit number to watch for
0, // limit polarity to wait for (1 or 0)
100.0, // speed to move while searching for index
-1, // index bit number to watch for (use -1 for none)
0, // index polarity to wait for (1 or 0)
200); // amount to move inside limits
EnableAxisDest(4,0.0); // cero maquina en X
//MDI("G49");
//DoPCFloat(PC_COMM_SET_X,0.0);
if (result==0)
{
printf("Home X succeded\n");
}
else
{
printf("Home X failed\n");
return;
}
// HOME Y
result = SimpleHomeIndexFunction(5, // axis number to home
5000.0, // speed to move toward home
-1, // direction to move toward home (+1 or -1)
136, // limit bit number to watch for
0, // limit polarity to wait for (1 or 0)
100.0, // speed to move while searching for index
-1, // index bit number to watch for (use -1 for none)
1, // index polarity to wait for (1 or 0)
200); // amount to move inside limits
EnableAxisDest(5,0.0); // cero maquina en Y
if (result==0)
{
printf("Home Y succeded\n");
}
else
{
printf("Home Y failed\n");
return;
}
ch4->LimitSwitchOptions=0x0;
ch4->SoftLimitPos=500165;
ch4->SoftLimitNeg=-500;
ch5->LimitSwitchOptions=0x0;
ch5->SoftLimitPos=578500;
ch5->SoftLimitNeg=-500;
DoPC(PC_COMM_HALT); // stop KMotionCNC
}
on this one the X and Y DRO reads as it should zero in machine position (red numbers)
Code: Select all
#define TMP 10 // which spare persist to use to transfer data
#include "KflopToKMotionCNCFunctions.c"
// Example Init program that includes "smooth" MPG motion example
// which makes use of the exponential motion command.
#define QA 27 // define to which IO bits the AB signals are connected
#define QB 26
#define ESTOP_BIT 16//shorting to pin 3, 3.3v to pin 5 bit 16
#define TAU 0.08 // smoothness factor (Low Pass Time constant seconds)
#define FINAL_TIME 1.0 // Set final dest after this amount of time with no change
#define SELECTX 28
#define SELECTY 29
#define SELECTZ 30
#define SELECTA 31
#define SELECTB 32
#define SELECTC 33
#define FACTOR1 21
#define FACTOR10 34
#define FACTOR100 35
// variables calculo visualizador velocidad hilo en pantalla
void ServiceTacoRpm(void);
#define V_HILO 9.166 // velocidad de salida del hilo en mm por revolucion del motor
#define VOLT 3 // voltaje a velocidad maxima del tacometro
#define CYCLE_TIME 0.3 //seconds between velocity estimations
#define VEL 170 // usa esta variable en el DROlabel para mostrar la velocidad de giro
char TS[80];
// variables y definicion de funciones control comunicacion pc-kflop
void ServiceWatchdogStatus(void);
void WatchdogTripped(void);
void WatchdogOK(void);
#define WATCHDOG_DELAY 5.0 // seconds after host stops to Trigger
main()
{
int BitA,Change1=0,Change2=0, DiffX2;
int PosNoWrap, NewPos, Pos=0, wraps;
int InMotion=FALSE,Axis,LastAxis=-1;
double LastChangeTime=0,Target,Factor=0;
FPGA(STEP_PULSE_LENGTH_ADD) = 32 + 0x40; // set the pulse time to ~ 2µs and multiplex to JP4 and JP6
//FPGA(STEP_PULSE_LENGTH_ADD) = 63 + 0x80; // set polarity and pulse length to 4us
ch0->InputMode=NO_INPUT_MODE;
ch0->OutputMode=STEP_DIR_MODE;
ch0->Vel=3000;
ch0->Accel=60000;
ch0->Jerk=4e+006;
ch0->P=0;
ch0->I=0.01;
ch0->D=0;
ch0->FFAccel=0;
ch0->FFVel=0;
ch0->MaxI=200;
ch0->MaxErr=1e+006;
ch0->MaxOutput=200;
ch0->DeadBandGain=1;
ch0->DeadBandRange=0;
ch0->InputChan0=0;
ch0->InputChan1=0;
ch0->OutputChan0=8;
ch0->OutputChan1=0;
ch0->MasterAxis=-1;
ch0->LimitSwitchOptions=0x0;
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=1000000000;
ch0->StepperAmplitude=20;
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.000769;
ch0->iir[2].B1=0.001538;
ch0->iir[2].B2=0.000769;
ch0->iir[2].A1=1.92076;
ch0->iir[2].A2=-0.923833;
EnableAxisDest(0,ch0->Dest);
ch1->InputMode=NO_INPUT_MODE;
ch1->OutputMode=STEP_DIR_MODE;
ch1->Vel=4000;
ch1->Accel=2000;
ch1->Jerk=4e+006;
ch1->P=0;
ch1->I=0.01;
ch1->D=0;
ch1->FFAccel=0;
ch1->FFVel=0;
ch1->MaxI=200;
ch1->MaxErr=1e+006;
ch1->MaxOutput=200;
ch1->DeadBandGain=1;
ch1->DeadBandRange=0;
ch1->InputChan0=0;
ch1->InputChan1=0;
ch1->OutputChan0=9;
ch1->OutputChan1=0;
ch1->MasterAxis=-1;
ch1->LimitSwitchOptions=0x0;
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=1000000000;
ch1->StepperAmplitude=20;
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.000769;
ch1->iir[2].B1=0.001538;
ch1->iir[2].B2=0.000769;
ch1->iir[2].A1=1.92076;
ch1->iir[2].A2=-0.923833;
EnableAxisDest(1,ch1->Dest);
ch4->InputMode=NO_INPUT_MODE;
ch4->OutputMode=STEP_DIR_MODE;
ch4->Vel=4000;
ch4->Accel=28000;
ch4->Jerk=4e+006;
ch4->P=0;
ch4->I=0.01;
ch4->D=0;
ch4->FFAccel=0;
ch4->FFVel=0;
ch4->MaxI=200;
ch4->MaxErr=1e+006;
ch4->MaxOutput=200;
ch4->DeadBandGain=1;
ch4->DeadBandRange=0;
ch4->InputChan0=4;
ch4->InputChan1=0;
ch4->OutputChan0=12;
ch4->OutputChan1=0;
ch4->MasterAxis=-1;
ch4->LimitSwitchOptions=0x0;
ch4->SoftLimitPos=1e+008;
ch4->SoftLimitNeg=-1e+008;
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=1000000000;
ch4->StepperAmplitude=20;
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=0.000769;
ch4->iir[2].B1=0.001538;
ch4->iir[2].B2=0.000769;
ch4->iir[2].A1=1.92076;
ch4->iir[2].A2=-0.923833;
EnableAxisDest(4,ch4->Dest);
ch5->InputMode=NO_INPUT_MODE;
ch5->OutputMode=STEP_DIR_MODE;
ch5->Vel=4000;
ch5->Accel=40000;
ch5->Jerk=4e+006;
ch5->P=0;
ch5->I=0.01;
ch5->D=0;
ch5->FFAccel=0;
ch5->FFVel=0;
ch5->MaxI=200;
ch5->MaxErr=1e+006;
ch5->MaxOutput=200;
ch5->DeadBandGain=1;
ch5->DeadBandRange=0;
ch5->InputChan0=4;
ch5->InputChan1=0;
ch5->OutputChan0=13;
ch5->OutputChan1=0;
ch5->MasterAxis=-1;
ch5->LimitSwitchOptions=0x0;
ch5->SoftLimitPos=1e+009;
ch5->SoftLimitNeg=-1e+009;
ch5->InputGain0=-1;
ch5->InputGain1=1;
ch5->InputOffset0=0;
ch5->InputOffset1=0;
ch5->OutputGain=-1;
ch5->OutputOffset=0;
ch5->SlaveGain=1;
ch5->BacklashMode=BACKLASH_LINEAR;
ch5->BacklashAmount=55.62;
ch5->BacklashRate=500;
ch5->invDistPerCycle=1;
ch5->Lead=0;
ch5->MaxFollowingError=1000000000;
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=0.000769;
ch5->iir[2].B1=0.001538;
ch5->iir[2].B2=0.000769;
ch5->iir[2].A1=1.92076;
ch5->iir[2].A2=-0.923833;
EnableAxisDest(5,ch5->Dest);
ch6->InputMode=NO_INPUT_MODE;
ch6->OutputMode=STEP_DIR_MODE;
ch6->Vel=4000;
ch6->Accel=40000;
ch6->Jerk=4e+006;
ch6->P=0;
ch6->I=0.01;
ch6->D=0;
ch6->FFAccel=0;
ch6->FFVel=0;
ch6->MaxI=200;
ch6->MaxErr=1e+006;
ch6->MaxOutput=200;
ch6->DeadBandGain=1;
ch6->DeadBandRange=0;
ch6->InputChan0=6;
ch6->InputChan1=0;
ch6->OutputChan0=14;
ch6->OutputChan1=0;
ch6->MasterAxis=-1;
ch6->LimitSwitchOptions=0x0;
ch6->SoftLimitPos=1e+009;
ch6->SoftLimitNeg=-1e+009;
ch6->InputGain0=-1;
ch6->InputGain1=1;
ch6->InputOffset0=0;
ch6->InputOffset1=0;
ch6->OutputGain=1;
ch6->OutputOffset=0;
ch6->SlaveGain=1;
ch6->BacklashMode=BACKLASH_OFF;
ch6->BacklashAmount=0;
ch6->BacklashRate=0;
ch6->invDistPerCycle=1;
ch6->Lead=0;
ch6->MaxFollowingError=1000000000;
ch6->StepperAmplitude=20;
ch6->iir[0].B0=1;
ch6->iir[0].B1=0;
ch6->iir[0].B2=0;
ch6->iir[0].A1=0;
ch6->iir[0].A2=0;
ch6->iir[1].B0=1;
ch6->iir[1].B1=0;
ch6->iir[1].B2=0;
ch6->iir[1].A1=0;
ch6->iir[1].A2=0;
ch6->iir[2].B0=0.000769;
ch6->iir[2].B1=0.001538;
ch6->iir[2].B2=0.000769;
ch6->iir[2].A1=1.92076;
ch6->iir[2].A2=-0.923833;
EnableAxisDest(6,ch6->Dest);
ch7->InputMode=NO_INPUT_MODE;
ch7->OutputMode=STEP_DIR_MODE;
ch7->Vel=4000;
ch7->Accel=40000;
ch7->Jerk=4e+006;
ch7->P=0;
ch7->I=0.01;
ch7->D=0;
ch7->FFAccel=0;
ch7->FFVel=0;
ch7->MaxI=200;
ch7->MaxErr=1e+006;
ch7->MaxOutput=200;
ch7->DeadBandGain=1;
ch7->DeadBandRange=0;
ch7->InputChan0=6;
ch7->InputChan1=0;
ch7->OutputChan0=15;
ch7->OutputChan1=0;
ch7->MasterAxis=-1;
ch7->LimitSwitchOptions=0x0;
ch7->SoftLimitPos=1e+009;
ch7->SoftLimitNeg=-1e+009;
ch7->InputGain0=-1;
ch7->InputGain1=1;
ch7->InputOffset0=0;
ch7->InputOffset1=0;
ch7->OutputGain=-1;
ch7->OutputOffset=0;
ch7->SlaveGain=1;
ch7->BacklashMode=BACKLASH_OFF;
ch7->BacklashAmount=0;
ch7->BacklashRate=0;
ch7->invDistPerCycle=1;
ch7->Lead=0;
ch7->MaxFollowingError=1000000000;
ch7->StepperAmplitude=20;
ch7->iir[0].B0=1;
ch7->iir[0].B1=0;
ch7->iir[0].B2=0;
ch7->iir[0].A1=0;
ch7->iir[0].A2=0;
ch7->iir[1].B0=1;
ch7->iir[1].B1=0;
ch7->iir[1].B2=0;
ch7->iir[1].A1=0;
ch7->iir[1].A2=0;
ch7->iir[2].B0=0.000769;
ch7->iir[2].B1=0.001538;
ch7->iir[2].B2=0.000769;
ch7->iir[2].A1=1.92076;
ch7->iir[2].A2=-0.923833;
EnableAxisDest(7,ch7->Dest);
DefineCoordSystem6(4,5,0,6,7,1);
for (;;)
{
// convert quadrature to 2 bit binary
BitA = ReadBit(QA);
PosNoWrap = (ReadBit(QB) ^ BitA) | (BitA<<1);
// Diff between expected position based on average of two prev deltas
// and position with no wraps. (Keep as X2 to avoid division by 2)
// DiffX2 = 2*(Pos-PosNoWrap) + (Change2+Change1);
DiffX2 = 2*(Pos-PosNoWrap);
// Calc quadrature wraparounds to bring Diff nearest zero
// offset by 128 wraps to avoid requiring floor()
wraps = ((DiffX2+1028)>>3)-128;
// factor in the quadrature wraparounds
NewPos = PosNoWrap + (wraps<<2);
Change2 = Change1;
Change1 = NewPos - Pos;
Pos = NewPos;
if (ReadBit(FACTOR1)) // is X1 selected?
Factor = 0.04;
else if (ReadBit(FACTOR10)) // is X10 selected?
Factor = 0.4;
else if (ReadBit(FACTOR100)) // is X100 selected?
Factor = 4.0;
else
Factor = 0.0;
if (ReadBit(SELECTX)) // is x selected?
Axis=4;
else if (ReadBit(SELECTY)) // is y selected?
Axis=5;
else if (ReadBit(SELECTZ)) // is z selected?
Axis=0;
else if (ReadBit(SELECTA)) // is A selected?
Axis=6;
else if (ReadBit(SELECTB)) // is y selected?
Axis=7;
else if (ReadBit(SELECTC)) // is y selected?
Axis=1;
else
Axis=-1;
// check if the Axis just changed or we have been
// converging to the target for a long time
if (Axis != LastAxis ||
(InMotion && Time_sec() > LastChangeTime+FINAL_TIME))
{
if (InMotion)
Move(LastAxis,Target); //finalize any motion
LastAxis = Axis;
InMotion = FALSE;
}
if (Change1) // did we move?
{
if (!InMotion) Target = chan[Axis].Dest;
Target += Change1 * Factor;
MoveExp(Axis,Target,TAU); // note: contains a WaitNextTimeSlice
LastChangeTime = Time_sec();
InMotion=TRUE;
}
else
{
WaitNextTimeSlice();
}
if (ReadBit(16)) // estop??
{
DisableAxis(0);
DisableAxis(1);
DisableAxis(4);
DisableAxis(5);
DisableAxis(6);
DisableAxis(7);
ClearBit(160);
ClearBit(144);
ClearBit(145);
ClearBit(146);
ClearBit(147);
}
WaitNextTimeSlice();
ServiceTacoRpm();
ServiceWatchdogStatus();
}
return 0;
}
// FUNCION MUESTRA AVANCE HILO EN PANTALLA
void ServiceTacoRpm(void)
{
static double T0=0.0; // remember the last time we turned on
double Q=Time_sec(); // get current Time_sec
if (T0==0.0 || Q > T0 + CYCLE_TIME)
{
double P1 = KANALOG_CONVERT_ADC_TO_VOLTS(ADC(4)); // Read current position
sprintf(TS,"%.1f dm/min\n", P1 * (100.99*VOLT/V_HILO)); // Display RPM
DROLabel(200, VEL, TS); // mandamos los datos a la pantalla
T0=Q; // set start time for the next cycle
}
}
// FUNCIONES QUE CONTROLAN LA COMUNICACION PC-KFLOP
void ServiceWatchdogStatus(void)
{
static int Alive=FALSE;
static int PrevStatusRequestCounter=-1;
static double WatchdogTime=0;
double T=Time_sec();
// check if Host is requesting Status
if (StatusRequestCounter != PrevStatusRequestCounter)
{
// yes, save time
WatchdogTime = T + WATCHDOG_DELAY;
PrevStatusRequestCounter=StatusRequestCounter;
if (!Alive) WatchdogOK();
Alive=TRUE;
}
else
{
if (T > WatchdogTime) // time to trigger?
{
if (Alive) WatchdogTripped();
Alive=FALSE;
}
}
}
void WatchdogOK(void)
{
/*
ClearBit(47); // KFLOP LED off
Delay_sec(1);
SetBit(47);
Delay_sec(1);
*/
}
void WatchdogTripped(void)
{
DisableAxis(0);
DisableAxis(1);
DisableAxis(4);
DisableAxis(5);
DisableAxis(6);
DisableAxis(7);
ClearBit(160);
ClearBit(144);
ClearBit(145);
ClearBit(146);
ClearBit(147);
}
#include "KMotionDef.h"
#include "SimpleHomeIndexFunction.c"
#define TMP 10
#include "KflopToKMotionCNCFunctions.c"
//#include "KFLOPtoPCCmdExamples.c"
// home subroutine
main()
{
int result;
ch4->LimitSwitchOptions=0x0;
ch4->SoftLimitPos=1e30;
ch4->SoftLimitNeg=-1e30;
ch5->LimitSwitchOptions=0x0;
ch5->SoftLimitPos=1e30;
ch5->SoftLimitNeg=-1e30;
// HOME X
result = SimpleHomeIndexFunction(4, // axis number to home
2000.0, // speed to move toward home
-1, // direction to move toward home (+1 or -1)
137, // limit bit number to watch for
0, // limit polarity to wait for (1 or 0)
100.0, // speed to move while searching for index
-1, // index bit number to watch for (use -1 for none)
0, // index polarity to wait for (1 or 0)
200); // amount to move inside limits
EnableAxisDest(4,0.0); // cero maquina en X
//MDI("G49");
//DoPCFloat(PC_COMM_SET_X,0.0);
if (result==0)
{
printf("Home X succeded\n");
}
else
{
printf("Home X failed\n");
return;
}
// HOME Y
result = SimpleHomeIndexFunction(5, // axis number to home
5000.0, // speed to move toward home
-1, // direction to move toward home (+1 or -1)
136, // limit bit number to watch for
0, // limit polarity to wait for (1 or 0)
100.0, // speed to move while searching for index
-1, // index bit number to watch for (use -1 for none)
1, // index polarity to wait for (1 or 0)
200); // amount to move inside limits
EnableAxisDest(5,0.0); // cero maquina en Y
if (result==0)
{
printf("Home Y succeded\n");
}
else
{
printf("Home Y failed\n");
return;
}
ch4->LimitSwitchOptions=0x0;
ch4->SoftLimitPos=500165;
ch4->SoftLimitNeg=-500;
ch5->LimitSwitchOptions=0x0;
ch5->SoftLimitPos=578500;
ch5->SoftLimitNeg=-500;
DoPC(PC_COMM_HALT); // stop KMotionCNC
}
- TomKerekes
- Posts: 2635
- Joined: Mon Dec 04, 2017 1:49 am
Re: DRO Not Zero After Homing
Are you saying after a power cycle on KFLOP and only launching KMotionCNC the XY DROs show 25.4?
Regards,
Tom Kerekes
Dynomotion, Inc.
Tom Kerekes
Dynomotion, Inc.
Re: DRO Not Zero After Homing
Hi Tom;
I turn on the computer, at this time the kflop has no power( have a toggle switch that is off when the computer is booting .
when the computer finish the booting sequence, then I start Kmotion CNC , at this time the DRO shows
X 0000 Y 0000, in yellow numbers, also at this time the Kflop is off.
Then I apply power to the Kflop, and after the kflop finish the booting sequence the readout changes to
X25.4000 Y 25.4000, still in yellow numbers , because the initialization program is not running yet.
if I turn off the power at this time to the kflop, a message will pop up saying Read Failed ,auto disconnet
thanks for your help
Mariano
I turn on the computer, at this time the kflop has no power( have a toggle switch that is off when the computer is booting .
when the computer finish the booting sequence, then I start Kmotion CNC , at this time the DRO shows
X 0000 Y 0000, in yellow numbers, also at this time the Kflop is off.
Then I apply power to the Kflop, and after the kflop finish the booting sequence the readout changes to
X25.4000 Y 25.4000, still in yellow numbers , because the initialization program is not running yet.
if I turn off the power at this time to the kflop, a message will pop up saying Read Failed ,auto disconnet
thanks for your help
Mariano