Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp
/*
 * Aligns ABN encoders by forcing an angle with high current and calculating the offset
 */
Aligns ABN encoders by forcing an angle with high current and calculating the offset
Aligns ABN encoders by forcing an angle with high current and calculating the offset
void TMC4671::bangInitEnc(int16_t power){
	if(!hasPower() || (this->conf.motconf.motor_type != MotorType::STEPPER && this->conf.motconf.motor_type != MotorType::BLDC)){ 
		return;
	}
	blinkClipLed(50, 0);
	PhiE lastphie = getPhiEtype();
	MotionMode lastmode = getMotionMode();
	setPhiE_ext(0);
	setPhiEtype(PhiE::ext);
	uint8_t phiEreg = 0;
	uint8_t phiEoffsetReg = 0;
	if(conf.motconf.enctype == EncoderType_TMC::abn){
		phiEreg = 0x2A;
		phiEoffsetReg = 0x29;
		writeReg(0x27,0); 
	}else if(conf.motconf.enctype == EncoderType_TMC::sincos || conf.motconf.enctype == EncoderType_TMC::uvw){
		phiEreg = 0x46;
		writeReg(0x41,0); 
		writeReg(0x47,0); 
		phiEoffsetReg = 0x45;
	}
	setPos(0);
	updateReg(phiEoffsetReg, 0, 0xffff, 16); 
	int16_t phiEpos = 0; 
	setPhiE_ext(phiEpos);
	for(int16_t flux = 0; flux <= power; flux+=10){
		setFluxTorque(flux, 0);
		Delay(3);
	}
	int16_t phiE_enc = readReg(phiEreg)>>16;
	Delay(100);
	int16_t phiE_abn_old = 0;
	int16_t c = 0;
	uint16_t still = 0;
	while(still < 30 && c++ < 4000){
		if(abs(phiE_enc - phiE_abn_old) < 200){
			still++;
		}else{
			still = 0;
		}
		phiE_abn_old = phiE_enc;
		phiE_enc=readReg(phiEreg)>>16;
		Delay(10);
	}
	abnconf.phiEoffset = phiEpos-phiE_enc;
	updateReg(phiEoffsetReg, abnconf.phiEoffset, 0xffff, 16);
	setFluxTorque(0, 0);
	setPhiE_ext(0);
	setPhiEtype(lastphie);
	setMotionMode(lastmode,true);
	setPos(0);
	blinkClipLed(0, 0);
}
void TMC4671 :: bangInitEnc ( int16_t power ) { if ( ! hasPower ( ) || ( this -> conf . motconf . motor_type != MotorType :: STEPPER && this -> conf . motconf . motor_type != MotorType :: BLDC ) ) { return ; } blinkClipLed ( 50 , 0 ) ; PhiE lastphie = getPhiEtype ( ) ; MotionMode lastmode = getMotionMode ( ) ; setPhiE_ext ( 0 ) ; setPhiEtype ( PhiE :: ext ) ; uint8_t phiEreg = 0 ; uint8_t phiEoffsetReg = 0 ; if ( conf . motconf . enctype == EncoderType_TMC :: abn ) { phiEreg = 0x2A ; phiEoffsetReg = 0x29 ; writeReg ( 0x27 , 0 ) ; } else if ( conf . motconf . enctype == EncoderType_TMC :: sincos || conf . motconf . enctype == EncoderType_TMC :: uvw ) { phiEreg = 0x46 ; writeReg ( 0x41 , 0 ) ; writeReg ( 0x47 , 0 ) ; phiEoffsetReg = 0x45 ; } setPos ( 0 ) ; updateReg ( phiEoffsetReg , 0 , 0xffff , 16 ) ; int16_t phiEpos = 0 ; setPhiE_ext ( phiEpos ) ; for ( int16_t flux = 0 ; flux <= power ; flux += 10 ) { setFluxTorque ( flux , 0 ) ; Delay ( 3 ) ; } int16_t phiE_enc = readReg ( phiEreg ) >> 16 ; Delay ( 100 ) ; int16_t phiE_abn_old = 0 ; int16_t c = 0 ; uint16_t still = 0 ; while ( still < 30 && c ++ < 4000 ) { if ( abs ( phiE_enc - phiE_abn_old ) < 200 ) { still ++ ; } else { still = 0 ; } phiE_abn_old = phiE_enc ; phiE_enc = readReg ( phiEreg ) >> 16 ; Delay ( 10 ) ; } abnconf . phiEoffset = phiEpos - phiE_enc ; updateReg ( phiEoffsetReg , abnconf . phiEoffset , 0xffff , 16 ) ; setFluxTorque ( 0 , 0 ) ; setPhiE_ext ( 0 ) ; setPhiEtype ( lastphie ) ; setMotionMode ( lastmode , true ) ; setPos ( 0 ) ; blinkClipLed ( 0 , 0 ) ; }
Aligns ABN encoders by forcing an angle with high current and calculating the offset
Aligns ABN encoders by forcing an angle with high current and calculating the offset
// If not stepper or bldc return //setFluxTorque(power, 0); //int32_t pos = getPos(); //Zero encoder //Zero encoder //Zero encoder // Set phiE offset to zero //setMotionMode(MotionMode::uqudext); //Delay(100); // This is where the check starts too // Ramp up flux // Wait for motor to stop moving //Write offset //int16_t phiE_abn = readReg(0x2A)>>16; //setPos(pos+getPos());