Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp
// Rotates motor to find min and max values of the encoder
Rotates motor to find min and max values of the encoder
Rotates motor to find min and max values of the encoder
void TMC4671::calibrateAenc(){
		offset += std::sqrt((aencUX+aencWY) / 2);
		samples++;
	}
	offset = offset/samples;
	aencconf.AENC0_offset = offset;
	aencconf.AENC1_offset = offset;
	aencconf.AENC2_offset = offset;
	setup_AENC(aencconf);
	blinkClipLed(0, 0);
	 Rotate and measure min/max
	blinkClipLed(250, 0);
	PhiE lastphie = getPhiEtype();
	MotionMode lastmode = getMotionMode();
	int32_t pos = getPos();
	PosSelection possel = this->conf.motconf.pos_sel;
	setPosSel(PosSelection::PhiE_openloop);
	setPos(0);
	 Ramp up flux
	setFluxTorque(0, 0);
	writeReg(0x23,0);  set phie openloop 0
	setPhiEtype(PhiE::openloop);
	setMotionMode(MotionMode::torque,true);
	if(this->conf.motconf.motor_type == MotorType::STEPPER || this->conf.motconf.motor_type == MotorType::BLDC)
		for(int16_t flux = 0; flux <= bangInitPower; flux+=10){
			setFluxTorque(flux, 0);
			Delay(10);
		}
	uint32_t minVal_0 = 0xffff,	minVal_1 = 0xffff,	minVal_2 = 0xffff;
	uint32_t maxVal_0 = 0,	maxVal_1 = 0,	maxVal_2 = 0;
	int32_t minpos = -0x8fff/std::max<int32_t>(1,std::min<int32_t>(this->aencconf.cpr/4,20)), maxpos = 0x8fff/std::max<int32_t>(1,std::min<int32_t>(this->aencconf.cpr/4,20));
	uint32_t speed = std::max<uint32_t>(1,20/std::max<uint32_t>(1,this->aencconf.cpr/10));
	if(this->conf.motconf.motor_type != MotorType::STEPPER && this->conf.motconf.motor_type != MotorType::BLDC){
		speed*=10;  dc motors turn at a random speed. reduce the rotation time a bit by increasing openloop speed
	}
	runOpenLoop(bangInitPower, 0, speed, 100,true);
	uint8_t stage = 0;
	int32_t poles = conf.motconf.pole_pairs;
	int32_t initialDirPos = 0;
	while(stage != 3){
		Delay(2);
		if(getPos() > maxpos*poles && stage == 0){
			runOpenLoop(bangInitPower, 0, -speed, 100,true);
			stage = 1;
		}else if(getPos() < minpos*poles && stage == 1){
			 Scale might still be wrong... maxVal-minVal is too high. In theory its 0xffff range and scaler /256. Leave some room to prevent clipping
			aencconf.AENC0_offset = ((maxVal_0 + minVal_0) / 2);
			aencconf.AENC0_scale = 0xF6FF00 / (maxVal_0 - minVal_0);
			if(conf.motconf.enctype == EncoderType_TMC::uvw){
				aencconf.AENC1_offset = ((maxVal_1 + minVal_1) / 2);
				aencconf.AENC1_scale = 0xF6FF00 / (maxVal_1 - minVal_1);
			}
			aencconf.AENC2_offset = ((maxVal_2 + minVal_2) / 2);
			aencconf.AENC2_scale = 0xF6FF00 / (maxVal_2 - minVal_2);
			aencconf.rdir = false;
			setup_AENC(aencconf);
			runOpenLoop(0, 0, 0, 1000,true);
			Delay(250);
			 Zero aenc
			writeReg(0x41, 0);
			initialDirPos = readReg(0x41);
			runOpenLoop(bangInitPower, 0, speed, 100,true);
			stage = 2;
		}else if(getPos() > 0 && stage == 2){
			stage = 3;
			runOpenLoop(0, 0, 0, 1000,true);
		}
		writeReg(0x03,2);
		uint32_t aencUX = readReg(0x02)>>16;
		writeReg(0x03,3);
		uint32_t aencWY_VN = readReg(0x02) ;
		uint32_t aencWY = aencWY_VN >> 16;
		uint32_t aencVN = aencWY_VN & 0xffff;
		minVal_0 = std::min(minVal_0,aencUX);
		minVal_1 = std::min(minVal_1,aencVN);
		minVal_2 = std::min(minVal_2,aencWY);
		maxVal_0 = std::max(maxVal_0,aencUX);
		maxVal_1 = std::max(maxVal_1,aencVN);
		maxVal_2 = std::max(maxVal_2,aencWY);
	}
	 Scale is not actually important. but offset must be perfect
	aencconf.AENC0_offset = ((maxVal_0 + minVal_0) / 2);
	aencconf.AENC0_scale = 0xF6FF00 / (maxVal_0 - minVal_0);
	if(conf.motconf.enctype == EncoderType_TMC::uvw){
		aencconf.AENC1_offset = ((maxVal_1 + minVal_1) / 2);
		aencconf.AENC1_scale = 0xF6FF00 / (maxVal_1 - minVal_1);
	}
	aencconf.AENC2_offset = ((maxVal_2 + minVal_2) / 2);
	aencconf.AENC2_scale = 0xF6FF00 / (maxVal_2 - minVal_2);
	int32_t newDirPos = readReg(0x41);
	aencconf.rdir =  (initialDirPos - newDirPos) > 0;
	setup_AENC(aencconf);
	 Restore settings
	setPhiEtype(lastphie);
	setMotionMode(lastmode,true);
	setPosSel(possel);
	setPos(0);
	blinkClipLed(0, 0);
}
void TMC4671 :: calibrateAenc ( ) { blinkClipLed ( 250 , 0 ) ; PhiE lastphie = getPhiEtype ( ) ; MotionMode lastmode = getMotionMode ( ) ; PosSelection possel = this -> conf . motconf . pos_sel ; setPosSel ( PosSelection :: PhiE_openloop ) ; setPos ( 0 ) ; setFluxTorque ( 0 , 0 ) ; writeReg ( 0x23 , 0 ) ; setPhiEtype ( PhiE :: openloop ) ; setMotionMode ( MotionMode :: torque , true ) ; if ( this -> conf . motconf . motor_type == MotorType :: STEPPER || this -> conf . motconf . motor_type == MotorType :: BLDC ) for ( int16_t flux = 0 ; flux <= bangInitPower ; flux += 10 ) { setFluxTorque ( flux , 0 ) ; Delay ( 10 ) ; } uint32_t minVal_0 = 0xffff , minVal_1 = 0xffff , minVal_2 = 0xffff ; uint32_t maxVal_0 = 0 , maxVal_1 = 0 , maxVal_2 = 0 ; int32_t minpos = -0x8fff / std :: max < int32_t > ( 1 , std :: min < int32_t > ( this -> aencconf . cpr / 4 , 20 ) ) , maxpos = 0x8fff / std :: max < int32_t > ( 1 , std :: min < int32_t > ( this -> aencconf . cpr / 4 , 20 ) ) ; uint32_t speed = std :: max < uint32_t > ( 1 , 20 / std :: max < uint32_t > ( 1 , this -> aencconf . cpr / 10 ) ) ; if ( this -> conf . motconf . motor_type != MotorType :: STEPPER && this -> conf . motconf . motor_type != MotorType :: BLDC ) { speed *= 10 ; } runOpenLoop ( bangInitPower , 0 , speed , 100 , true ) ; uint8_t stage = 0 ; int32_t poles = conf . motconf . pole_pairs ; int32_t initialDirPos = 0 ; while ( stage != 3 ) { Delay ( 2 ) ; if ( getPos ( ) > maxpos * poles && stage == 0 ) { runOpenLoop ( bangInitPower , 0 , - speed , 100 , true ) ; stage = 1 ; } else if ( getPos ( ) < minpos * poles && stage == 1 ) { aencconf . AENC0_offset = ( ( maxVal_0 + minVal_0 ) / 2 ) ; aencconf . AENC0_scale = 0xF6FF00 / ( maxVal_0 - minVal_0 ) ; if ( conf . motconf . enctype == EncoderType_TMC :: uvw ) { aencconf . AENC1_offset = ( ( maxVal_1 + minVal_1 ) / 2 ) ; aencconf . AENC1_scale = 0xF6FF00 / ( maxVal_1 - minVal_1 ) ; } aencconf . AENC2_offset = ( ( maxVal_2 + minVal_2 ) / 2 ) ; aencconf . AENC2_scale = 0xF6FF00 / ( maxVal_2 - minVal_2 ) ; aencconf . rdir = false ; setup_AENC ( aencconf ) ; runOpenLoop ( 0 , 0 , 0 , 1000 , true ) ; Delay ( 250 ) ; writeReg ( 0x41 , 0 ) ; initialDirPos = readReg ( 0x41 ) ; runOpenLoop ( bangInitPower , 0 , speed , 100 , true ) ; stage = 2 ; } else if ( getPos ( ) > 0 && stage == 2 ) { stage = 3 ; runOpenLoop ( 0 , 0 , 0 , 1000 , true ) ; } writeReg ( 0x03 , 2 ) ; uint32_t aencUX = readReg ( 0x02 ) >> 16 ; writeReg ( 0x03 , 3 ) ; uint32_t aencWY_VN = readReg ( 0x02 ) ; uint32_t aencWY = aencWY_VN >> 16 ; uint32_t aencVN = aencWY_VN & 0xffff ; minVal_0 = std :: min ( minVal_0 , aencUX ) ; minVal_1 = std :: min ( minVal_1 , aencVN ) ; minVal_2 = std :: min ( minVal_2 , aencWY ) ; maxVal_0 = std :: max ( maxVal_0 , aencUX ) ; maxVal_1 = std :: max ( maxVal_1 , aencVN ) ; maxVal_2 = std :: max ( maxVal_2 , aencWY ) ; } aencconf . AENC0_offset = ( ( maxVal_0 + minVal_0 ) / 2 ) ; aencconf . AENC0_scale = 0xF6FF00 / ( maxVal_0 - minVal_0 ) ; if ( conf . motconf . enctype == EncoderType_TMC :: uvw ) { aencconf . AENC1_offset = ( ( maxVal_1 + minVal_1 ) / 2 ) ; aencconf . AENC1_scale = 0xF6FF00 / ( maxVal_1 - minVal_1 ) ; } aencconf . AENC2_offset = ( ( maxVal_2 + minVal_2 ) / 2 ) ; aencconf . AENC2_scale = 0xF6FF00 / ( maxVal_2 - minVal_2 ) ; int32_t newDirPos = readReg ( 0x41 ) ; aencconf . rdir = ( initialDirPos - newDirPos ) > 0 ; setup_AENC ( aencconf ) ; setPhiEtype ( lastphie ) ; setMotionMode ( lastmode , true ) ; setPosSel ( possel ) ; setPos ( 0 ) ; blinkClipLed ( 0 , 0 ) ; }
Rotates motor to find min and max values of the encoder
Rotates motor to find min and max values of the encoder
// static offset estimation. not exact //	blinkClipLed(250, 0); //	uint32_t offset = 0; //	uint32_t samples = 0; //	for(uint16_t i = 0;i<500;i++){ //		Delay(1); //		writeReg(0x03,2); //		uint32_t aencUX = readReg(0x02)>>16; //		aencUX *= aencUX; //		writeReg(0x03,3); //		uint32_t aencWY_VN = readReg(0x02) ; //		uint32_t aencWY = aencWY_VN >> 16; //		aencWY *= aencWY; //		uint32_t aencVN = aencWY_VN & 0xffff; //		aencVN *= aencVN; // //		offset += std::sqrt((aencUX+aencWY) / 2); //		samples++; //	} //	offset = offset/samples; //	aencconf.AENC0_offset = offset; //	aencconf.AENC1_offset = offset; //	aencconf.AENC2_offset = offset; //	setup_AENC(aencconf); //	blinkClipLed(0, 0); // Rotate and measure min/max //int32_t pos = getPos(); // Ramp up flux // set phie openloop 0 // dc motors turn at a random speed. reduce the rotation time a bit by increasing openloop speed // Scale might still be wrong... maxVal-minVal is too high. In theory its 0xffff range and scaler /256. Leave some room to prevent clipping // Zero aenc // Scale is not actually important. but offset must be perfect // Restore settings