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); }