main()
{
int i=0;
double t0,t1,t2,t3;
for (;;)
{
t0=WaitNextTimeSlice();
t1=WaitNextTimeSlice();
t3=Time_sec();
do
{
t2=t3;
t3=Time_sec();
}
while (t3-t2 < 10e-6) ;
if (i==0)
{
printf("Time Slice = %8.2f us\n",(t1-t0)*0.5e6);
printf("Thread Execution time = %8.2f us\n",(t2-t1)*1e6);
i++;
}
printf("IRQ time = %8.2f us\n",((t1-t0)*0.5-(t2-t1))*1e6);
Delay_sec(1);
}
}
main()
{
int i,k;
double T0,*p=gather_buffer;
while (CS0_TimeExecuted==0.0) ; // wait till we Start
t0 = Time_sec();
// Capture Data
for (i=0; i<N; i++)
{
for (k=0; k<4; k++) WaitNextTimeSlice();
*p++ = Time_sec() - T0;
*p++ = ch0->Dest;
*p++ = ch0->Position;
*p++ = ch1->Dest;
*p++ = ch1->Position;
}
for (i=0; i<N; i++)
{
for (k=0; k<4; k++) WaitNextTimeSlice();
*p++ = Time_sec() - T0;
*p++ = ch0->Dest;
*p++ = ch0->Position;
*p++ = ch1->Dest;
*p++ = ch1->Position;
}
}
void ServiceCSS(void)
{
float rpm;
double T=Time_sec();
if (*css_mode == 2 && T > css_T) // check if we are in CSS mode and it is time to update
{
css_T=T+CSS_UPDATE_DT; // determine next time to update
// convert axis position to distance from center in inches
float radius = fast_fabs((chan[CS0_axis_x].Dest - *css_xoff) * *css_xfactor);
if (radius > 0.0f)
rpm = *css_s / (radius * (TWO_PI_F/60.0f));
else
rpm = *css_max_rpm;
if (rpm > *css_max_rpm) rpm = *css_max_rpm;
if (persist.UserData[STATEVAR]!=0) // if spindle is already on, ramp to new speed
Jog(SPINDLEAXIS,rpm * FACTOR);
// printf("xoff=%f radius= %f xfactor=%f s=%f(ips) maxrpm=%f rpm=%f\n",*css_xoff,radius,*css_xfactor,*css_s,*css_max_rpm,rpm);
}
}
void ServiceMaxOutput(void)
{
static BOOL FirstTime=TRUE;
static int i;
static double T0, X0, Z0;
double T1,X1,Z1,DX,DZ,iDT;
if (FirstTime) // first time initialize all
{
T0=Time_sec();
X0=ch0->Position;
Z0=ch1->Position;
i=0;
FirstTime=FALSE;
return;
}
if (++i < TIME_SLICES) return; // wait a few time slices
T1=Time_sec();
iDT=1.0/(T1-T0); // compute inverse delta time
T0=T1;
X1=ch0->Position; // compute delta X
DX=X1-X0;
X0=X1;
Z1=ch1->Position; // compute delta Z
DZ=Z1-Z0;
Z0=Z1;
if (DX>0.0) // X going forward ?
ch0->MaxOutput = DX * iDT * (X_OUTPUT/X_SPEED) + X_ALLOWED;
else
ch0->MaxOutput = DX * iDT * (X_OUTPUT/X_SPEED) - X_ALLOWED;
if (DZ>0.0) // Z going forward ?
ch1->MaxOutput = DZ * iDT * (Z_OUTPUT/Z_SPEED) + Z_ALLOWED;
else
ch1->MaxOutput = DZ * iDT * (Z_OUTPUT/Z_SPEED) - Z_ALLOWED;
i=0; // reset time slice counter
}
// Called before every servo Sample
void CallBack(void)
{
// Compute Error and Time Delay it
ch0->DestOffset = TimeDelay(ch0->Dest-ch0->Position)*GAIN;
// Force no Damping if Stopped (Integrator off) or User on/off
if (ch0->I < 1e-6f || persist.UserData[100]==0.0) ch0->DestOffset=0.0;
// maintain Dither mode
T1=Time_sec(); // returns current time
DoDither(ch0, P_original_0, P_low_0, I_original_0, &T0_0, T1);
}
// Services Some Operator buttons to perform two functions
// A short duration push of a single button will cause a tool change
// A long duration push of a single button will set the current turret
// position as that tool number
void ServiceToolButtons(void)
{
int NewButton=CheckForOneButtonPushed();
double T=Time_sec();
switch (ButtonState)
{
case T_IDLE:
{
// Single button pushed?
if (NewButton >= 0)
{
LastButton=NewButton;
ToolButtonTime=T;
ButtonState = T_ONE_BUTTON_PUSHED;
}
break;
}
case T_ONE_BUTTON_PUSHED:
{
// same one still pushed?
if (NewButton != LastButton)
{
// no, check for more than one pushed
if (NewButton==-2)
{
// more than one, do nothing and start over
}
// check how long it had been pushed
else if (T > ToolButtonTime + LONGPUSH)
{
*LastTool = LastButton+1; // set tool number
printf("Turret Set as Tool %d\n",*LastTool);
}
else if (T > ToolButtonTime + SHORTPUSH)
{
// short push, initiate a ToolChange if
// Tool Changer is idle
if (*ChangerState == T_IDLE)
{
*Tool = LastButton+1; // set new tool to load
*ChangerState = T_START; // Go!
printf("Tool %d Change Initiated\n",*Tool);
}
}
ButtonState = T_IDLE; // go back to idle state
}
break;
}
}
}
main()
{
int i,k;
double T0,*p=gather_buffer;
// I just have it go, launched by an M code. It records roughly 4 seconds from launch and
// doesn't appear to saturate the USB line. This lets me really target where I want it to
// log data such that I'm seeing the exact problem area. It displays axis position and
// following error!
T0 = Time_sec();
// Capture Data
for (i=0; i<N-1; i++)
{
for (k=0; k<4; k++) WaitNextTimeSlice();
*p++ = Time_sec() - T0;
*p++ = ch0->Position;
*p++ = ch1->Position;
*p++ = ch2->Position;
*p++ = ch0->Dest;
*p++ = ch1->Dest;
*p++ = ch2->Dest;
}
p=gather_buffer;
for (i=0; i<N; i++)
{
printf("%12.6f,%12.3f,%12.3f,%12.3f,%12.3f,%12.3f,%12.3f\n",p[0],p[1],p[2],p[3],p[4],p[5],p[6]);
p += 7;
}
}
void ServiceKonnectPWM(void)
{
static int FirstTime=TRUE;
static float Vc=0.0f;
static double T0;
static int State;
double T=Time_sec();
if (FirstTime)
{
FirstTime=FALSE;
T0=T;
State=0;
}
else
{
float V,I;
float RPM=*(float *)&persist.UserData[KMVAR];
float Vout = RPM*(5.0 / 1000.0);
// Compute Voltage applied to Cap
V=Vcc*State;
// Compute current
I=(V-Vc)/R;
// Compute new Cap Voltage
Vc += I/C*(T-T0);
// determine next state
if (Vc > Vout)
{
ClearBit(HIGH_BIT);
SetBit(LOW_BIT);
State=0;
}
else
{
ClearBit(LOW_BIT);
SetBit(HIGH_BIT);
State=1;
}
T0=T; // save time when applied
}
}
int main()
{
double T0, LastX=0, LastY=0, LastZ=0, Tau;
KStepPresent=TRUE; // enable KSTEP input multiplexing
FPGA(KAN_TRIG_REG)=4; // Mux PWM0 to JP7 Pin5 IO 44 for KSTEP
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=40000;
ch0->Accel=200000;
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+030;
ch0->SoftLimitNeg=-1e+030;
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,0);
ch1->InputMode=NO_INPUT_MODE;
ch1->OutputMode=STEP_DIR_MODE;
ch1->Vel=40000;
ch1->Accel=200000;
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+030;
ch1->SoftLimitNeg=-1e+030;
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,0);
ch2->InputMode=NO_INPUT_MODE;
ch2->OutputMode=STEP_DIR_MODE;
ch2->Vel=40000;
ch2->Accel=200000;
ch2->Jerk=4e+006;
ch2->P=0;
ch2->I=0.01;
ch2->D=0;
ch2->FFAccel=0;
ch2->FFVel=0;
ch2->MaxI=200;
ch2->MaxErr=1e+006;
ch2->MaxOutput=200;
ch2->DeadBandGain=1;
ch2->DeadBandRange=0;
ch2->InputChan0=0;
ch2->InputChan1=0;
ch2->OutputChan0=10;
ch2->OutputChan1=0;
ch2->MasterAxis=-1;
ch2->LimitSwitchOptions=0x0;
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=1000000000;
ch2->StepperAmplitude=20;
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=1;
ch2->iir[2].B1=0;
ch2->iir[2].B2=0;
ch2->iir[2].A1=0;
ch2->iir[2].A2=0;
EnableAxisDest(2,0);
DefineCoordSystem(0,1,2,-1);
SetBitDirection(45,1); // set Enable Signal as Output
SetBit(45); // Enable the amplifiers
// Add a small amount of Coordinated Motion Path smoothing if desired
// Tau = 0.001; // seconds for Low Pass Filter Time Constant
// KLP = exp(-TIMEBASE/Tau);
KLP=0; // force to 0 to disable
// printf("Tau=%f KLP=%f\n",Tau,KLP);
for (;;) // loop forever
{
WaitNextTimeSlice();
// Service Amplifier disable after no activity for a while
if (ch0->Dest != LastX || ch1->Dest != LastY || ch2->Dest != LastZ)
{
// we moved - enable KStep Amplifers
SetBit(45);
T0 = Time_sec(); // record the time and position of last motion
LastX=ch0->Dest;
LastY=ch1->Dest;
LastZ=ch2->Dest;
}
else
{
if (Time_sec() > T0 + 10.0) ClearBit(45);
}
}
return 0;
}
void DoEllipse(void)
{
int i,ibeg,iend;
double dist_beg,dist_end,V0,ta,da,length,Time0,Time1,MaxAp,MaxVp,angle;
Time0=Time_sec();
// first compute total length of path and build tables of info
length = ArcLength(ThetaStart,ThetaEnd,MAXN);
Time1=Time_sec();
printf("Length = %.9f Compute Time %.0f us\n",length, (Time1-Time0)*1e6);
// time to achieve max vel
ta = MaxV/MaxA;
// dist to achieve max vel
da = (0.5 * MaxA * ta) * ta;
printf("Accel Time %f Accel dist %f\n",ta,da);
// first move from where we are to starting point on ellipse
x0=ch0->Dest; // starting point is where we currently are
y0=ch1->Dest;
z0=ch2->Dest;
a0=ch3->Dest;
x1 = pointx[0]*RESX;
y1 = pointy[0]*RESY;
z1=z0; // keep z and a the same
a1=a0;
DoLinear();
// now rotate knife to be at the right angle for the first segment
angle = FindAngle(pointx[1]-pointx[0],pointy[1]-pointy[0]);
printf("angle=%f %f %f\n",angle,pointx[1]-pointx[0],pointy[1]-pointy[0]);
x0=x1; // xyz doesn't move
y0=y1;
z0=z1;
a0=a1;
a1=angle*RESA; // rotate knife to proper angle
DoLinear();
// search to find segments needed to accellerate/decelerate
// searches forward and backward throug the segments simultaneously
// until either the begiining and ending are sufficient to accel/deccel
// or we meet in the middle and run out of distance
ibeg=0;
iend=MAXN;
dist_beg = dist_end = 0.0;
do
{
if (dist_beg < dist_end)
ibeg++;
else
iend--;
dist_beg = sum[ibeg];
dist_end = sum[MAXN]-sum[iend];
// printf("ibeg=%d iend=%d dist beg %.3f dist_end %.3f\n",ibeg,iend,dist_beg,dist_end);
}
while ((dist_beg<da || dist_end<da) && ibeg<iend);
// if distance available is less than required accel distance
// then reduce the max velocity to what is achievable
if (ibeg>=iend)
{
if (dist_beg<dist_end)
MaxVp=sqrt(2.0*MaxA*dist_beg);
else
MaxVp=sqrt(2.0*MaxA*dist_end);
}
else
{
MaxVp=MaxV;
}
// adjust the acceleration to accelerate in exactly the beginning distance
MaxAp = 0.5*MaxVp*MaxVp/dist_beg;
// create all the segments
f=fopen("C:\\temp\\kflopdata.txt","wt");
V0=0.0; // start from stop
for (i=1;i<=ibeg;i++)
DoSegment(i,&V0,MaxAp); // accel segments
for (;i<=iend;i++)
DoSegment(i,&V0,0.0); // constant velocity
// adjust the acceleration to accelerate in exactly the ending distance
MaxAp = 0.5*MaxVp*MaxVp/dist_end;
for (;i<=MAXN;i++)
DoSegment(i,&V0,-MaxAp); // decel segments
fclose(f);
}
void CallBack(void)
{
T0=Time_sec();
Delta = T0-T1;
T1=T0;
}
