从Arduino摘取一段步进电机角度与电流闭环控制核心代码

int32_t FetDriver::move(int32_t stepAngle, uint32_t mA)
{
	uint16_t angle;
	int32_t cos,sin;
	int32_t dacSin,dacCos;
	int32_t dacSin_mA,dacCos_mA;
	int32_t maxMa;
	static int32_t last_dacSin_mA=0,last_dacCos_mA=0;;
	if (enabled == false)
	{
		WARNING("FET Driver disabled");

		//turn the current off to FETs
		coilA_PWM(0);
		coilB_PWM(0);

		//float the FET outputs by disabling FET driver.
		GPIO_LOW(PIN_FET_ENABLE);
		return stepAngle;
	}
	GPIO_HIGH(PIN_FET_ENABLE);


	maxMa=NVM->motorParams.currentMa;
	if (maxMa==0)
	{
		maxMa=2200;
	}

	//WARNING("move %d %d",stepAngle,mA);
	//handle roll overs, could do with modulo operator
	//stepAngle=stepAngle%SINE_STEPS;
	//	while (stepAngle<0)
	//	{
	//		stepAngle=stepAngle+SINE_STEPS;
	//	}
	//	while (stepAngle>=SINE_STEPS)
	//	{
	//		stepAngle=stepAngle-SINE_STEPS;
	//	}

	//figure out our sine Angle
	// note our SINE_STEPS is 4x of microsteps for a reason
	//angle=(stepAngle+(SINE_STEPS/8)) % SINE_STEPS;
	angle=(stepAngle) % SINE_STEPS;
	//calculate the sine and cosine of our angle
	sin=sine(angle);
	cos=cosine(angle);

	//if we are reverse swap the sign of one of the angels
	if (false == forwardRotation)
	{
		cos=-cos;
	}

	//LOG("sin/cos %d %d %d", sin,cos,angle);
	//scale sine result by current(mA)
	dacSin_mA=((int32_t)mA*(int32_t)(sin))/SINE_MAX;

	//scale cosine result by current(mA)
	dacCos_mA=((int32_t)mA*(int32_t)(cos))/SINE_MAX;

	coilA_SetPoint=FET_MA_TO_ADC(dacSin_mA);
	coilB_SetPoint=FET_MA_TO_ADC(dacCos_mA);
	//LOG("sin/cos %d %d", dacSin,dacCos);

	//convert value into 12bit DAC scaled to 3300mA max
	dacSin=(int32_t)((int64_t)dacSin_mA*(255))/maxMa;

	//convert value into 12bit DAC scaled to 3300mA max
	dacCos=(int32_t)((int64_t)dacCos_mA*(255))/maxMa;

	//LOG("sin/cos %d %d", dacSin,dacCos);
	//limit the table index to +/-255
	dacCos=MIN(dacCos,(int32_t)255);
	dacCos=MAX(dacCos,(int32_t)-255);
	dacSin=MIN(dacSin,(int32_t)255);
	dacSin=MAX(dacSin,(int32_t)-255);


	if ((dacSin_mA-last_dacSin_mA)>200)
	{
		GPIO_LOW(PIN_FET_IN2);
		PIN_GPIO_OUTPUT(PIN_FET_IN2);
		GPIO_HIGH(PIN_FET_IN1);
		PIN_GPIO_OUTPUT(PIN_FET_IN1);
	}else if ((dacSin_mA-last_dacSin_mA)<-200)
	{
		GPIO_HIGH(PIN_FET_IN2);
		PIN_GPIO_OUTPUT(PIN_FET_IN2);
		GPIO_LOW(PIN_FET_IN1);
		PIN_GPIO_OUTPUT(PIN_FET_IN1);
	}

	if ((dacCos_mA-last_dacCos_mA)>200)
	{
		GPIO_LOW(PIN_FET_IN4);
		PIN_GPIO_OUTPUT(PIN_FET_IN4);
		GPIO_HIGH(PIN_FET_IN3);
		PIN_GPIO_OUTPUT(PIN_FET_IN3);
	}else if ((dacCos_mA-last_dacCos_mA)<-200)
	{
		GPIO_HIGH(PIN_FET_IN4);
		PIN_GPIO_OUTPUT(PIN_FET_IN4);
		GPIO_LOW(PIN_FET_IN3);
		PIN_GPIO_OUTPUT(PIN_FET_IN3);
		}
	delayMicroseconds(20);
	last_dacSin_mA=dacSin_mA;
	last_dacCos_mA=dacCos_mA;

//	YELLOW_LED(1);
//	uint32_t t0=micros();
//	int done=0;
//	int32_t a,b;
//	a=FET_MA_TO_ADC(dacSin_mA);
//	b=FET_MA_TO_ADC(dacCos_mA);
//	while ((micros()-t0)<20 && done!=0x03)
//	{
//		if ( (fastADCRead(ISENSE_FET_A)-a)<FET_MA_TO_ADC(200))
//		{
//			GPIO_LOW(PIN_FET_IN2);
//			PIN_GPIO_OUTPUT(PIN_FET_IN2);
//			GPIO_HIGH(PIN_FET_IN1);
//			PIN_GPIO_OUTPUT(PIN_FET_IN1);
//			//coilA_PWM(PWM_Table_A[dacSin+255]);
//			done |=0x01;
//		}
//
//		if ((fastADCRead(ISENSE_FET_A)-a)>FET_MA_TO_ADC(200))
//		{
//			GPIO_HIGH(PIN_FET_IN2);
//			PIN_GPIO_OUTPUT(PIN_FET_IN2);
//			GPIO_LOW(PIN_FET_IN1);
//			PIN_GPIO_OUTPUT(PIN_FET_IN1);
//			done |=0x01;
//		}
//		if  ((fastADCRead(ISENSE_FET_B)-b)<FET_MA_TO_ADC(200))
//		{
//			GPIO_LOW(PIN_FET_IN4);
//			PIN_GPIO_OUTPUT(PIN_FET_IN4);
//			GPIO_HIGH(PIN_FET_IN3);
//			PIN_GPIO_OUTPUT(PIN_FET_IN3);
//			done |=0x02;
//		}
//		if  ((fastADCRead(ISENSE_FET_B)-b)>FET_MA_TO_ADC(200))
//		{
//			GPIO_HIGH(PIN_FET_IN4);
//			PIN_GPIO_OUTPUT(PIN_FET_IN4);
//			GPIO_LOW(PIN_FET_IN3);
//			PIN_GPIO_OUTPUT(PIN_FET_IN3);
//			done |=0x02;
//		}
//
//	}
//
//	YELLOW_LED(0);


	//LOG("sin/cos %d %d", dacSin,dacCos);
	//loop up the current from table and set the PWM
	coilA_PWM(PWM_Table_A[dacSin+255]);
	coilB_PWM(PWM_Table_B[dacCos+255]);

	lastStepMicros=micros();
	return stepAngle;
}