Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp
/*
 * Steps the motor a few times to check if the encoder follows correctly
 */
Steps the motor a few times to check if the encoder follows correctly
Steps the motor a few times to check if the encoder follows correctly
bool TMC4671::checkEncoder(){
	if(this->conf.motconf.motor_type != MotorType::STEPPER && this->conf.motconf.motor_type != MotorType::BLDC){ 
		return true;
	}
	blinkClipLed(150, 0);
	uint8_t phiEreg = 0;
	if(conf.motconf.enctype == EncoderType_TMC::abn){
		phiEreg = 0x2A;
	}else if(conf.motconf.enctype == EncoderType_TMC::sincos || conf.motconf.enctype == EncoderType_TMC::uvw){
		phiEreg = 0x46;
	}
	const uint16_t maxcount = 50;
	const int16_t startAngle = 0;
	const int16_t targetAngle = 0x3FFF;
	bool result = true;
	PhiE lastphie = getPhiEtype();
	MotionMode lastmode = getMotionMode();
	setFluxTorque(0, 0);
	setPhiEtype(PhiE::ext);
	setPhiE_ext(startAngle);
	for(int16_t flux = 0; flux <= bangInitPower; flux+=20){
		setFluxTorque(flux, 0);
		Delay(2);
	}
	int16_t phiE_enc = 0;
	uint16_t failcount = 0;
	int16_t revCount = 0;
	for(int16_t angle = startAngle;angle<targetAngle;angle+=0x00ff){
		uint16_t c = 0;
		setPhiE_ext(angle);
		Delay(10);
		phiE_enc = (int16_t)(readReg(phiEreg)>>16);
		int16_t err = abs(phiE_enc - angle);
		int16_t nErr = abs(phiE_enc + angle);
		while(err > 2500 && nErr > 2500 && c++ < 50){
			phiE_enc = (int16_t)(readReg(phiEreg)>>16);
			err = abs(phiE_enc - angle);
			nErr = abs(angle - phiE_enc);
			Delay(10);
		}
		if(err > nErr){
			revCount++;
		}
		if(c >= maxcount){
			failcount++;
			if(failcount > 10){
				result = false;
				break;
			}
		}
	}
	if(result){ 
		for(int16_t angle = targetAngle;angle>startAngle;angle -= 0x00ff){
			uint16_t c = 0;
			setPhiE_ext(angle);
			Delay(10);
			phiE_enc = (int16_t)(readReg(phiEreg)>>16);
			int16_t err = abs(phiE_enc - angle);
			int16_t nErr = abs(phiE_enc + angle);
			while(err > 2500 && nErr > 2500 && c++ < 50){
				phiE_enc = (int16_t)(readReg(phiEreg)>>16);
				err = abs(phiE_enc - angle);
				nErr = abs(angle - phiE_enc);
				Delay(10);
			}
			if(err > nErr){
				revCount++;
			}
			if(c >= maxcount){
				failcount++;
				if(failcount > 10){
					result = false;
					break;
				}
			}
		}
	}
	if(revCount > maxcount){ 
		if(this->conf.motconf.enctype == EncoderType_TMC::abn){
			this->abnconf.rdir = !this->abnconf.rdir;
			setup_ABN_Enc(abnconf);
		}else if(this->conf.motconf.enctype == EncoderType_TMC::sincos || this->conf.motconf.enctype == EncoderType_TMC::uvw){
			this->aencconf.rdir = !this->aencconf.rdir;
			setup_AENC(aencconf);
		}
	}
	setFluxTorque(0, 0);
	setPhiE_ext(0);
	setPhiEtype(lastphie);
	setMotionMode(lastmode,true);
	if(result){
		encstate = ENC_InitState::OK;
	}
	blinkClipLed(0, 0);
	return result;
}
bool TMC4671 :: checkEncoder ( ) { if ( this -> conf . motconf . motor_type != MotorType :: STEPPER && this -> conf . motconf . motor_type != MotorType :: BLDC ) { return true ; } blinkClipLed ( 150 , 0 ) ; uint8_t phiEreg = 0 ; if ( conf . motconf . enctype == EncoderType_TMC :: abn ) { phiEreg = 0x2A ; } else if ( conf . motconf . enctype == EncoderType_TMC :: sincos || conf . motconf . enctype == EncoderType_TMC :: uvw ) { phiEreg = 0x46 ; } const uint16_t maxcount = 50 ; const int16_t startAngle = 0 ; const int16_t targetAngle = 0x3FFF ; bool result = true ; PhiE lastphie = getPhiEtype ( ) ; MotionMode lastmode = getMotionMode ( ) ; setFluxTorque ( 0 , 0 ) ; setPhiEtype ( PhiE :: ext ) ; setPhiE_ext ( startAngle ) ; for ( int16_t flux = 0 ; flux <= bangInitPower ; flux += 20 ) { setFluxTorque ( flux , 0 ) ; Delay ( 2 ) ; } int16_t phiE_enc = 0 ; uint16_t failcount = 0 ; int16_t revCount = 0 ; for ( int16_t angle = startAngle ; angle < targetAngle ; angle += 0x00ff ) { uint16_t c = 0 ; setPhiE_ext ( angle ) ; Delay ( 10 ) ; phiE_enc = ( int16_t ) ( readReg ( phiEreg ) >> 16 ) ; int16_t err = abs ( phiE_enc - angle ) ; int16_t nErr = abs ( phiE_enc + angle ) ; while ( err > 2500 && nErr > 2500 && c ++ < 50 ) { phiE_enc = ( int16_t ) ( readReg ( phiEreg ) >> 16 ) ; err = abs ( phiE_enc - angle ) ; nErr = abs ( angle - phiE_enc ) ; Delay ( 10 ) ; } if ( err > nErr ) { revCount ++ ; } if ( c >= maxcount ) { failcount ++ ; if ( failcount > 10 ) { result = false ; break ; } } } if ( result ) { for ( int16_t angle = targetAngle ; angle > startAngle ; angle -= 0x00ff ) { uint16_t c = 0 ; setPhiE_ext ( angle ) ; Delay ( 10 ) ; phiE_enc = ( int16_t ) ( readReg ( phiEreg ) >> 16 ) ; int16_t err = abs ( phiE_enc - angle ) ; int16_t nErr = abs ( phiE_enc + angle ) ; while ( err > 2500 && nErr > 2500 && c ++ < 50 ) { phiE_enc = ( int16_t ) ( readReg ( phiEreg ) >> 16 ) ; err = abs ( phiE_enc - angle ) ; nErr = abs ( angle - phiE_enc ) ; Delay ( 10 ) ; } if ( err > nErr ) { revCount ++ ; } if ( c >= maxcount ) { failcount ++ ; if ( failcount > 10 ) { result = false ; break ; } } } } if ( revCount > maxcount ) { if ( this -> conf . motconf . enctype == EncoderType_TMC :: abn ) { this -> abnconf . rdir = ! this -> abnconf . rdir ; setup_ABN_Enc ( abnconf ) ; } else if ( this -> conf . motconf . enctype == EncoderType_TMC :: sincos || this -> conf . motconf . enctype == EncoderType_TMC :: uvw ) { this -> aencconf . rdir = ! this -> aencconf . rdir ; setup_AENC ( aencconf ) ; } } setFluxTorque ( 0 , 0 ) ; setPhiE_ext ( 0 ) ; setPhiEtype ( lastphie ) ; setMotionMode ( lastmode , true ) ; if ( result ) { encstate = ENC_InitState :: OK ; } blinkClipLed ( 0 , 0 ) ; return result ; }
Steps the motor a few times to check if the encoder follows correctly
Steps the motor a few times to check if the encoder follows correctly
// If not stepper or bldc return // Ramp up flux //Forward // Wait more // Backward // Only if not already failed // Wait more // Encoder seems reversed // reverse encoder