1 everything off, no programs open
2 turn power on to servo drives and CH1 immediately holds position ( CH1 does not hold with no KFLOP input, only when connected )
3 run my master arm program and all 3 axis find position normally
Code: Select all
#include "KMotionDef.h"
// Home and set the commutation for a 3 phase brushless motor // that is driven using two DAC Outputs // // Phase rotates until index mark is located and Encoder Zeroed
// Function definitions
// x must be in the range 0->1 returns sin(x) and sin(x+120 degrees)
float Sine3PH (float x, float *sin2);
// put a voltage v on a 3 Phase motor at specified commutation angle
void Write3PH_DACs(CHAN *chx, float v, double angle_in_cycles);
// return fractional part of a huge number accurately
float fractionf(double v);
// Main function entry point
void main()
{
float k=0,A=500; // set coil current amplitude DAC units
double p0;
// Z axis
ch0->InputMode=ENCODER_MODE;
ch0->OutputMode=NO_OUTPUT_MODE;
ch0->Vel=400000;
ch0->Accel=10000000;
ch0->Jerk=200000000;
ch0->P=1;
ch0->I=0;
ch0->D=20;
ch0->FFAccel=0;
ch0->FFVel=0;
ch0->MaxI=200;
ch0->MaxErr=1e+006;
ch0->MaxOutput=2000;
ch0->DeadBandGain=1;
ch0->DeadBandRange=0;
ch0->InputChan0=0;
ch0->InputChan1=0;
ch0->OutputChan0=1;
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=-0.0004;
ch0->Lead=0;
ch0->MaxFollowingError=1000000;
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=1;
ch0->iir[2].B1=0;
ch0->iir[2].B2=0;
ch0->iir[2].A1=0;
ch0->iir[2].A2=0;
// X axis
ch1->InputMode=ENCODER_MODE;
ch1->OutputMode=NO_OUTPUT_MODE;
ch1->Vel=400000;
ch1->Accel=10000000;
ch1->Jerk=200000000;
ch1->P=1;
ch1->I=0;
ch1->D=20;
ch1->FFAccel=0;
ch1->FFVel=0;
ch1->MaxI=200;
ch1->MaxErr=1e+006;
ch1->MaxOutput=500;
ch1->DeadBandGain=1;
ch1->DeadBandRange=0;
ch1->InputChan0=1;
ch1->InputChan1=1;
ch1->OutputChan0=2;
ch1->OutputChan1=3;
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=0.0004;
ch1->Lead=0;
ch1->MaxFollowingError=1000000;
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=1;
ch1->iir[2].B1=0;
ch1->iir[2].B2=0;
ch1->iir[2].A1=0;
ch1->iir[2].A2=0;
// A axis / spindle
ch2->InputMode=ENCODER_MODE;
ch2->OutputMode=NO_OUTPUT_MODE;
ch2->Vel= 300000;
ch2->Accel=100000;
ch2->Jerk= 2000000;
ch2->P=10;
ch2->I=0;
ch2->D=20;
ch2->FFAccel=0;
ch2->FFVel=0;
ch2->MaxI=200;
ch2->MaxErr=1e+006;
ch2->MaxOutput=2000;
ch2->DeadBandGain=1;
ch2->DeadBandRange=0;
ch2->InputChan0=2;
ch2->InputChan1=2;
ch2->OutputChan0=5;
ch2->OutputChan1=4;
ch2->MasterAxis=-1;
ch2->LimitSwitchOptions=0x0;
ch2->SoftLimitPos=1e+030;
ch2->SoftLimitNeg=-1e+030;
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=0.0003662109375;
ch2->Lead=0;
ch2->MaxFollowingError=1000000;
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;
// rotate until we find the index mark
// Use f0 and f1 and f2 to flag when index found for ch0 and ch1 and ch2->
// Break out when both indexes found (f0=1 and f1=1 and f2=1)
ch0->Enable=FALSE;
ch1->Enable=FALSE;
ch2->Enable=FALSE;
int f0,f1,f2; // Index found flags
f0=0;f1=0;f2=0; // Set all to index not found
for (;;) //Indefinite loop
{
if (f0==1 && f1==1 && f2==1) break; // Done if all indexes found
//
if (f0==0) // If Ch0 index not found
{
WaitNextTimeSlice();
Write3PH_DACs(ch0,A, ++k/10000.0); // move the pole
if (ReadBit(36)) // check for index mark ch0
{
p0=ch0->Position; // Index found so save position
ch0->Position=0; // set current position to Zero
ch0->CommutationOffset = 600; // from auto phase find
printf("Position ch0 = %f\n",p0);
f0=1; // Mark as ch0 index found
} // End readbit
} // End if f0=0
//
if (f1==0) // If Ch1 index not found
{
WaitNextTimeSlice();
Write3PH_DACs(ch1,A, ++k/10000.0); // move the pole
if (ReadBit(37)) // check for index mark ch1
{
p0=ch1->Position; // Index found so save position
ch1->Position=0; // set current position to Zero
ch1->CommutationOffset = 225; // from auto phase find
printf("Position ch1 = %f\n",p0);
f1=1; // Mark as ch1 index found
} // End readbit
} // End f1=0
//
if (f2==0) // If Ch2 index not found
{
WaitNextTimeSlice();
Write3PH_DACs(ch2,A, ++k/10000.0); // move the pole
if (ReadBit(38)) // check for index mark ch2
{
p0=ch2->Position; // Index found so save position
ch2->Position=0; // set current position to Zero
ch2->CommutationOffset = 1920; // from auto phase find
printf("Position ch2 = %f\n",p0);
f2=1; // Mark as ch2 index found
} // End readbit
} // End f2=0
} // End for loop
EnableAxisDest(0,0.0f); // Enable servo at destination of 0 Z axis
EnableAxisDest(1,0.0f); // Enable servo at destination of 0 X axis
EnableAxisDest(2,0.0f); // Enable servo at destination of 0 A axis
DefineCoordSystem(1,-1,0,-1); // define which axes are in use
// Loop forever outputting Servo output to dual DAC_SERVO_MODE
for (;;)
{
WaitNextTimeSlice();
if (ch0->Enable) // if axis enabled commutate and output else command zero to DACs
Write3PH_DACs(ch0,ch0->Output, (ch0->Position + ch0->CommutationOffset) * ch0->invDistPerCycle);
else
Write3PH_DACs(ch0, 0.0,0.0);
if (ch1->Enable) // if axis enabled commutate and output else command zero to DACs
Write3PH_DACs(ch1,ch1->Output, (ch1->Position + ch1->CommutationOffset) * ch1->invDistPerCycle);
else
Write3PH_DACs(ch1, 0.0,0.0);
if (ch2->Enable) // if axis enabled commutate and output else command zero to DACs
Write3PH_DACs(ch2,ch2->Output, (ch2->Position + ch2->CommutationOffset) * ch2->invDistPerCycle);
else
Write3PH_DACs(ch2, 0.0,0.0);
} // End for loop
} // End main()
// put a voltage v on a 3 Phase motor at specified commutation angle
void Write3PH_DACs(CHAN *chx, float v, double angle_in_cycles)
{
float theta,sin1f;
if (angle_in_cycles<0)
theta = 1.0f-fractionf(-angle_in_cycles);
else
theta = fractionf(angle_in_cycles);
DAC(chx->OutputChan0, (int)(v * Sine3PH(theta,&sin1f)));
DAC(chx->OutputChan1, (int)(v * sin1f));
} // End Write3PH()
5 when i run the program from kmotion and hit "halt" CH0 and CH2 will disable but CH1 will not
6 when i run the program again but hit "stop" in kmotioncnc it will disable all the servos
picture of the trajectory plan
