Dynomotion Forum

Code: Select all

#include "KMotionDef.h"

// Drive a 3 Phase motor that has a Z index by
// 3 phase driving the coils (like a stepping motor)
// two revs each direction (two z pulses)
// monitor how many counts/cycle and counts/rev 
// (including direction)and also determine the commutation
// offset by recording the phase angle at the index 
// (which will be where zero is set) and offsetting by 1/4 of a cycle 
//
// Set the following parameters according to your situation
// See the report generated on the Console Screen

#define DAC_CHAN 0      // which pair of PWM channels used
#define ENCODER_CHAN 1  // which encoder we are connected to
#define ENCODER_GAIN -1  // Set to -1 if desired to reverse axis direction
#define AMPLITUDE 100   // Set how hard to drive the coils pwm counts
#define Z_BIT_NUMBER 36  // What bit the Z index is connected to
#define AXIS_CHAN 0     // Axis channel to be used and configured
#define Ncycles 4       // don't change this

// 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 *ch0, float v, double angle_in_cycles); 

// return fractional part of a huge number accurately
float fractionf(double v);


void main() 
{
	float mid,k=0,dk=0.2,A=AMPLITUDE;   // set coil current amplitude
	int i,ignore=300,kpos[Ncycles],zmark,m=0;
	double cnts_per_cycle,p0[Ncycles];
	CHAN *ch = &chan[AXIS_CHAN];

    
    
	// rotate until we find the index mark

	ch->Enable=FALSE;
	ch->InputMode=ENCODER_MODE;
	ch->InputChan0=ENCODER_CHAN;
	ch->OutputChan0=DAC_CHAN;
	ch->OutputChan1=DAC_CHAN+1;
	ch->OutputMode=NO_OUTPUT_MODE;
	ch->InputGain0=ENCODER_GAIN;
	
	

	for (;;)
	{
		WaitNextTimeSlice();

		k+= dk;
		

		Write3PH_DACs(ch,A, k/1000.0);  // move the pole 

		zmark = ReadBit(Z_BIT_NUMBER);

		if (!zmark && ignore>0) ignore--;

		if (ignore==0 && zmark)  // check for index mark
		{
			p0[m]=ch->Position;  // save position
			kpos[m]=k;           // save phase angle

			if (++m == Ncycles) 
			{
				ch->Position=0;  // set current position to Zero
				break;
			}

			if (m==2) dk = -dk;
			ignore=300;
		}
	}

	Write3PH_DACs(ch,0,0);    // turn off the coil    

	printf("\nREPORT\n------\n");
	for (i=0; i<Ncycles; i++)
		printf("%d Position = %6.0f PhaseAngle = %f\n",i,p0[i],kpos[i]/1000.0);

	printf("Counts per rev = %6.0f\n",p0[1]-p0[0]);
	cnts_per_cycle = (p0[1]-p0[0])/(kpos[1]-kpos[0])*1000.0;
	printf("Counts per cycle = %6.0f\n",cnts_per_cycle);

	// round to 16
	if (cnts_per_cycle>0)
		cnts_per_cycle = ((int)(cnts_per_cycle/16.0 + 0.5))*16.0;
	else
		cnts_per_cycle = ((int)(cnts_per_cycle/16.0 - 0.5))*16.0;

	printf("Counts per cycle (rounded to 16)= %6.0f\n",cnts_per_cycle);

	ch->invDistPerCycle = 1.0/cnts_per_cycle;
	printf("invDistPerCycle (rounded)= %15.12f\n",ch->invDistPerCycle);

	mid = (kpos[2]+kpos[1])/2000.0;
	mid = mid - (int)mid;
	ch->CommutationOffset = mid*cnts_per_cycle + 0.25*fast_fabs(cnts_per_cycle);
	printf("Commutation offset = %6.0f\n",ch->CommutationOffset);
	printf("Input Gain Specified = %6.3f\n",ch->InputGain0);
}


// put a voltage v on a 3 Phase motor at specified commutation angle
void Write3PH_DACs(CHAN *ch, 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(ch->OutputChan0, (int)(v * Sine3PH(theta,&sin1f)));
	DAC(ch->OutputChan1, (int)(v * sin1f));
}

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 *ch2, float v, double angle_in_cycles); 

// return fractional part of a huge number accurately
float fractionf(double v);

void main() 
{
	float k=0,A=200;   // set coil current amplitude DAC units
	double p0;

	// define the axis as 3 phase BRUSHLESS_3PH_MODE
	

	ch2->InputMode=ENCODER_MODE;
	ch2->OutputMode=NO_OUTPUT_MODE;
	ch2->Vel=500;
	ch2->Accel=50000;
	ch2->Jerk=300000;
	ch2->P=1;
	ch2->I=0;
	ch2->D=0;
	ch2->FFAccel=0;
	ch2->FFVel=0;
	ch2->MaxI=200;
	ch2->MaxErr=1e+006;
	ch2->MaxOutput=500;
	ch2->DeadBandGain=1;
	ch2->DeadBandRange=0;
	ch2->InputChan0=2;
	ch2->InputChan1=2;
	ch2->OutputChan0=0;
	ch2->OutputChan1=1;
	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

	ch2->Enable=FALSE;
	for (;;)
	{
		WaitNextTimeSlice();

		Write3PH_DACs(ch2,A, ++k/1000.0);  // move the pole 

		if (ReadBit(40))  // check for index mark
		{
			p0=ch2->Position; // save position
			ch2->Position=0;  // set current position to Zero

			ch2->CommutationOffset = 1920;  // from auto phase find

			printf("Position = %f\n",p0);
			break;
		}
	}

	
        EnableAxisDest(2,0.0f);  // Enable servo at destination of 0
	
	DefineCoordSystem(0,-1,1,2); // define which axes are in use

	// Loop forever outputting Servo output to dual DAC_SERVO_MODE

	for (;;)
	{
		CHAN *ch=ch2;

		if (ch->Enable)  // if axis enabled commutate and output else command zero to DACs
			Write3PH_DACs(ch, ch->Output, (ch->Position + ch->CommutationOffset) * ch->invDistPerCycle);		
		else
			Write3PH_DACs(ch, 0.0, 0.0);		
	}
}


// put a voltage v on a 3 Phase motor at specified commutation angle
void Write3PH_DACs(CHAN *ch, 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(ch->OutputChan0, (int)(v * Sine3PH(theta,&sin1f)));
	DAC(ch->OutputChan1, (int)(v * sin1f));
}