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;
}
main()
{
ch0->InputMode=NO_INPUT_MODE;
ch0->OutputMode=STEP_DIR_MODE;
ch0->Vel=STEPS_MM*400; // 400 mm/s;
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=0x100;
ch0->LimitSwitchNegBit=0;
ch0->LimitSwitchPosBit=0;
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=2.5;
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.000768788;
ch0->iir[2].B1=0.00153758;
ch0->iir[2].B2=0.000768788;
ch0->iir[2].A1=1.92076;
ch0->iir[2].A2=-0.923833;
ch1->InputMode=NO_INPUT_MODE;
ch1->OutputMode=STEP_DIR_MODE;
ch1->Vel=STEPS_MM*400; // 400 mm/s;
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=1;
ch1->InputChan1=0;
ch1->OutputChan0=9;
ch1->OutputChan1=0;
ch1->MasterAxis=-1;
ch1->LimitSwitchOptions=0x100;
ch1->LimitSwitchNegBit=0;
ch1->LimitSwitchPosBit=0;
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=2.5;
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.92081;
ch1->iir[2].A2=-0.923885;
ch2->InputMode=NO_INPUT_MODE;
ch2->OutputMode=STEP_DIR_MODE;
ch2->Vel=STEPS_MM*9; // 9 mm/s;
ch2->Accel=80000;
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=2;
ch2->InputChan1=0;
ch2->OutputChan0=10;
ch2->OutputChan1=0;
ch2->MasterAxis=-1;
ch2->LimitSwitchOptions=0x100;
ch2->LimitSwitchNegBit=0;
ch2->LimitSwitchPosBit=0;
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.25;
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=0.000769;
ch2->iir[2].B1=0.001538;
ch2->iir[2].B2=0.000769;
ch2->iir[2].A1=1.92081;
ch2->iir[2].A2=-0.923885;
EnableAxisDest(0,0);
EnableAxisDest(1,0);
EnableAxisDest(2,0);
DefineCoordSystem(0,1,2,-1);
//SetBitDirection(int bit, int dir);
SetBitDirection(2, 1);
SetBitDirection(4, 1);
SetBitDirection(3, 1);
return;
}
void main()
{
WriteSnapAmp(SNAP0+SNAP_PEAK_CUR_LIMIT0,9); // current limit
WriteSnapAmp(SNAP0+SNAP_PEAK_CUR_LIMIT1,9); // current limit
WriteSnapAmp(SNAP1+SNAP_PEAK_CUR_LIMIT0,9); // current limit
WriteSnapAmp(SNAP1+SNAP_PEAK_CUR_LIMIT1,9); // current limit
Delay_sec(0.1); // wait for any fault to clear
ch4->InputMode=NO_INPUT_MODE;
ch4->OutputMode=MICROSTEP_MODE;
ch4->Vel=400.000000;
ch4->Accel=5000.000000;
ch4->Jerk=50000.000000;
ch4->P=1.000000;
ch4->I=0.000000;
ch4->D=0.000000;
ch4->FFAccel=0.000000;
ch4->FFVel=0.000000;
ch4->MaxI=200.000000;
ch4->MaxErr=1000000.000000;
ch4->MaxOutput=200.000000;
ch4->DeadBandGain=1.000000;
ch4->DeadBandRange=0.000000;
ch4->InputChan0=4;
ch4->InputChan1=4;
ch4->OutputChan0=8;
ch4->OutputChan1=9;
ch4->LimitSwitchOptions=0x0;
ch4->InputGain0=1.000000;
ch4->InputGain1=1.000000;
ch4->InputOffset0=0.000000;
ch4->InputOffset1=0.000000;
ch4->invDistPerCycle=1.000000;
ch4->Lead=0.000000;
ch4->MaxFollowingError=1000000000.000000;
ch4->StepperAmplitude=60.000000;
ch4->iir[0].B0=1.000000;
ch4->iir[0].B1=0.000000;
ch4->iir[0].B2=0.000000;
ch4->iir[0].A1=0.000000;
ch4->iir[0].A2=0.000000;
ch4->iir[1].B0=1.000000;
ch4->iir[1].B1=0.000000;
ch4->iir[1].B2=0.000000;
ch4->iir[1].A1=0.000000;
ch4->iir[1].A2=0.000000;
ch4->iir[2].B0=1.000000;
ch4->iir[2].B1=0.000000;
ch4->iir[2].B2=0.000000;
ch4->iir[2].A1=0.000000;
ch4->iir[2].A2=0.000000;
ch5->InputMode=NO_INPUT_MODE;
ch5->OutputMode=MICROSTEP_MODE;
ch5->Vel=400.000000;
ch5->Accel=5000.000000;
ch5->Jerk=50000.000000;
ch5->P=1.000000;
ch5->I=0.000000;
ch5->D=0.000000;
ch5->FFAccel=0.000000;
ch5->FFVel=0.000000;
ch5->MaxI=200.000000;
ch5->MaxErr=1000000.000000;
ch5->MaxOutput=200.000000;
ch5->DeadBandGain=1.000000;
ch5->DeadBandRange=0.000000;
ch5->InputChan0=5;
ch5->InputChan1=5;
ch5->OutputChan0=10;
ch5->OutputChan1=11;
ch5->LimitSwitchOptions=0x0;
ch5->InputGain0=1.000000;
ch5->InputGain1=1.000000;
ch5->InputOffset0=0.000000;
ch5->InputOffset1=0.000000;
ch5->invDistPerCycle=1.000000;
ch5->Lead=0.000000;
ch5->MaxFollowingError=1000000000.000000;
ch5->StepperAmplitude=60.000000;
ch5->iir[0].B0=1.000000;
ch5->iir[0].B1=0.000000;
ch5->iir[0].B2=0.000000;
ch5->iir[0].A1=0.000000;
ch5->iir[0].A2=0.000000;
ch5->iir[1].B0=1.000000;
ch5->iir[1].B1=0.000000;
ch5->iir[1].B2=0.000000;
ch5->iir[1].A1=0.000000;
ch5->iir[1].A2=0.000000;
ch5->iir[2].B0=1.000000;
ch5->iir[2].B1=0.000000;
ch5->iir[2].B2=0.000000;
ch5->iir[2].A1=0.000000;
ch5->iir[2].A2=0.000000;
ch6->InputMode=NO_INPUT_MODE;
ch6->OutputMode=MICROSTEP_MODE;
ch6->Vel=400.000000;
ch6->Accel=5000.000000;
ch6->Jerk=50000.000000;
ch6->P=1.000000;
ch6->I=0.000000;
ch6->D=0.000000;
ch6->FFAccel=0.000000;
ch6->FFVel=0.000000;
ch6->MaxI=200.000000;
ch6->MaxErr=1000000.000000;
ch6->MaxOutput=200.000000;
ch6->DeadBandGain=1.000000;
ch6->DeadBandRange=0.000000;
ch6->InputChan0=4;
ch6->InputChan1=4;
ch6->OutputChan0=12;
ch6->OutputChan1=13;
ch6->LimitSwitchOptions=0x0;
ch6->InputGain0=1.000000;
ch6->InputGain1=1.000000;
ch6->InputOffset0=0.000000;
ch6->InputOffset1=0.000000;
ch6->invDistPerCycle=1.000000;
ch6->Lead=0.000000;
ch6->MaxFollowingError=1000000000.000000;
ch6->StepperAmplitude=60.000000;
ch6->iir[0].B0=1.000000;
ch6->iir[0].B1=0.000000;
ch6->iir[0].B2=0.000000;
ch6->iir[0].A1=0.000000;
ch6->iir[0].A2=0.000000;
ch6->iir[1].B0=1.000000;
ch6->iir[1].B1=0.000000;
ch6->iir[1].B2=0.000000;
ch6->iir[1].A1=0.000000;
ch6->iir[1].A2=0.000000;
ch6->iir[2].B0=1.000000;
ch6->iir[2].B1=0.000000;
ch6->iir[2].B2=0.000000;
ch6->iir[2].A1=0.000000;
ch6->iir[2].A2=0.000000;
DefineCoordSystem(4,5,6,-1);
EnableAxis(4);
EnableAxis(5);
EnableAxis(6);
}
void main()
{
float k=0,A=10.0f; // set coil current amplitude PWM units
double p0;
WriteSnapAmp(SNAP0+SNAP_SUPPLY_CLAMP0 ,SNAP_CONVERT_VOLTS_TO_ADC(80.0));
WriteSnapAmp(SNAP0+SNAP_SUPPLY_CLAMP_ENA0,1);
WriteSnapAmp(SNAP0+SNAP_PEAK_CUR_LIMIT0,9); // current limit
WriteSnapAmp(SNAP0+SNAP_PEAK_CUR_LIMIT1,9); // current limit
Delay_sec(1); // wait for any fault to clear
// rotate until we find the index mark
ch4->Enable=FALSE;
ch4->OutputChan0=8;
for (;;)
{
Delay_sec(0.001); // wait a millisecond
Write3PH(ch4,A, ++k/1000.0); // move the pole
if (ReadBit(68)) // check for index mark
{
p0=ch4->Position; // save position
ch4->Position=0; // set current position to Zero
// set commutation offset to 1/4th of a cycle
// encoder has 4000 counts/rev
// motor has 3 cycles per rev
ch4->CommutationOffset = 3*8000/2.0/12.0 * 1.02;
printf("Position = %f\n",p0);
break;
}
}
Write3PH(ch4,0,0); // turn off the coil
// define the axis as 3 phase BRUSHLESS_3PH_MODE
// and set low PID gains
ch4->InputMode=ENCODER_MODE;
ch4->OutputMode=BRUSHLESS_3PH_MODE;
ch4->Vel=100000.000000;
ch4->Accel=100000.000000;
ch4->Jerk=100000000.000000;
ch4->P=0.100000;
ch4->I=0.000000;
ch4->D=0.000000;
ch4->FFAccel=0.000000;
ch4->FFVel=0.000000;
ch4->MaxI=200.000000;
ch4->MaxErr=1000000.000000;
ch4->MaxOutput=230.000000;
ch4->DeadBandGain=1.000000;
ch4->DeadBandRange=0.000000;
ch4->InputChan0=4;
ch4->InputChan1=4;
ch4->OutputChan0=8;
ch4->OutputChan1=9;
ch4->LimitSwitchOptions=0x0;
ch4->InputGain0=1.000000;
ch4->InputGain1=1.000000;
ch4->InputOffset0=0.000000;
ch4->InputOffset1=0.000000;
ch4->invDistPerCycle=2.0/8000.0; // CW (2 cycles/rev) / (8000 encoder cnts/rev)
ch4->Lead=0.000000;
ch4->MaxFollowingError=4000.000000;
ch4->StepperAmplitude=20.000000;
ch4->iir[0].B0=1.000000;
ch4->iir[0].B1=0.000000;
ch4->iir[0].B2=0.000000;
ch4->iir[0].A1=0.000000;
ch4->iir[0].A2=0.000000;
ch4->iir[1].B0=1.000000;
ch4->iir[1].B1=0.000000;
ch4->iir[1].B2=0.000000;
ch4->iir[1].A1=0.000000;
ch4->iir[1].A2=0.000000;
ch4->iir[2].B0=1.000000;
ch4->iir[2].B1=0.000000;
ch4->iir[2].B2=0.000000;
ch4->iir[2].A1=0.000000;
ch4->iir[2].A2=0.000000;
EnableAxisDest(4,0.0f); // Enable servo at destination of 0
DefineCoordSystem(4,-1,-1,-1);
}
