TMC using flux ramping during init reducing sharp current changes

manoukianv · Aug 2, 2024 · 88489b9 · 88489b9
1 parent 290a181
commit 88489b9
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 29 deletions.
diff --git a/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h b/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h
@@ -458,6 +458,8 @@ friend class TMCDebugBridge;
 	int32_t getActualFlux();
 	int32_t getActualTorque();
 
+	void rampFlux(uint16_t target,uint16_t time_ms);
+
 	bool checkAdc();
 
 	float getTemp();

diff --git a/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp b/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp
@@ -839,16 +839,13 @@ bool TMC4671::findEncoderIndex(int32_t speed, uint16_t power,bool offsetPhiM,boo
 
 	//uint32_t mposStart = readReg(0x2A);
 	int32_t timeout = 1000; // 10s
-	for(int16_t flux = 0; flux <= power; flux+=1+power/250){
-		setFluxTorque(flux, 0);
-		Delay(3);
-	}
+	rampFlux(power, 500);
 	runOpenLoop(power, 0, speed, 10, true);
 	while(!encoderIndexHitFlag && timeout-- > 0){
 		Delay(10);
 	}
 	//int32_t speed = 10;
-
+	rampFlux(0, 100);
 	runOpenLoop(0, 0, 0, 10, true);
 	if(!encoderIndexHitFlag){
 		pulseErrLed();
@@ -987,10 +984,7 @@ void TMC4671::bangInitEnc(int16_t power){
 	setPhiE_ext(phiEpos);
 	setPhiEtype(PhiE::ext);
 	// Ramp up flux
-	for(int16_t flux = 0; flux <= power; flux+=1+power/250){
-		setFluxTorque(flux, 0);
-		Delay(3);
-	}
+	rampFlux(power, 1000);
 	int16_t phiE_enc = getPhiE_Enc();
 
 	Delay(50);
@@ -1011,7 +1005,7 @@ void TMC4671::bangInitEnc(int16_t power){
 		//phiE_enc=readReg(phiEreg)>>16;
 		Delay(10);
 	}
-	setFluxTorque(0, 0);
+	rampFlux(0, 100);
 
 	//Write offset
 	//int16_t phiE_abn = readReg(0x2A)>>16;
@@ -1062,10 +1056,7 @@ void TMC4671::calibrateAenc(){
 	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+=1+bangInitPower / 200){
-			setFluxTorque(flux, 0);
-			Delay(5);
-		}
+		rampFlux(bangInitPower, 250);
 	}
 	uint32_t minVal_0 = 0xffff,	minVal_1 = 0xffff,	minVal_2 = 0xffff;
 	uint32_t maxVal_0 = 0,	maxVal_1 = 0,	maxVal_2 = 0;
@@ -1099,6 +1090,7 @@ void TMC4671::calibrateAenc(){
 			aencconf.AENC2_scale = 0xF6FF00 / (maxVal_2 - minVal_2);
 			aencconf.rdir = false;
 			setup_AENC(aencconf);
+			rampFlux(0, 100);
 			runOpenLoop(0, 0, 0, 1000,true);
 			Delay(250);
 			// Zero aenc
@@ -1108,6 +1100,7 @@ void TMC4671::calibrateAenc(){
 			stage = 2;
 		}else if(getPos() > 0 && stage == 2){
 			stage = 3;
+			rampFlux(0, 100);
 			runOpenLoop(0, 0, 0, 1000,true);
 		}
 
@@ -1188,10 +1181,8 @@ bool TMC4671::checkEncoder(){
 
 	setPhiE_ext(startAngle);
 	// Ramp up flux
-	for(int16_t flux = 0; flux <= 2*bangInitPower/3; flux+=1+bangInitPower/250){
-		setFluxTorque(flux, 0);
-		Delay(2);
-	}
+	rampFlux(2*bangInitPower/3, 250);
+
 	//Forward
 	int16_t phiE_enc = 0;
 	uint16_t failcount = 0;
@@ -1279,7 +1270,7 @@ bool TMC4671::checkEncoder(){
 		ErrorHandler::addError(Error(ErrorCode::encoderReversed,ErrorType::warning,"Encoder direction reversed during check"));
 	}
 
-	setFluxTorque(0, 0);
+	rampFlux(0, 100);
 	setPhiE_ext(0);
 	setPhiEtype(lastphie);
 	setMotionMode(lastmode,true);
@@ -2063,6 +2054,22 @@ void TMC4671::setFluxTorqueFF(int16_t flux, int16_t torque){
 	writeReg(0x65, (flux & 0xffff) | (torque << 16));
 }
 
+/**
+ * Ramps flux from current value to a target value over a specified duration
+ */
+void TMC4671::rampFlux(uint16_t target,uint16_t time_ms){
+	uint16_t startFlux = readReg(0x64) & 0xffff;
+	int32_t stepsize = (target - startFlux) / std::max<uint16_t>(1, time_ms/2);
+	if(stepsize == 0){
+		stepsize = startFlux < target ? 1 : -1;
+	}
+	uint16_t flux = startFlux;
+	while(abs(target - flux) >= abs(stepsize)){
+		flux+=stepsize;
+		setFluxTorque(std::max<int32_t>(0,flux), 0);
+		Delay(2);
+	}
+}
 
 void TMC4671::setPids(TMC4671PIDConf pids){
 	curPids = pids;
@@ -2272,10 +2279,7 @@ void TMC4671::estimateABNparams(){
 	setPhiEtype(PhiE::ext);
 	setFluxTorque(0, 0);
 	setMotionMode(MotionMode::torque,true);
-	for(int16_t flux = 0; flux <= bangInitPower; flux+=1+bangInitPower/200){
-		setFluxTorque(flux, 0);
-		Delay(5);
-	}
+	rampFlux(bangInitPower, 1000);
 
 	int16_t phiE_abn = readReg(0x2A)>>16;
 	int16_t phiE_abn_old = 0;
@@ -2299,7 +2303,7 @@ void TMC4671::estimateABNparams(){
 	}
 	setTmcPos(pos+getPos());
 
-	setFluxTorque(0, 0);
+	rampFlux(0, 100);
 	setPhiEtype(lastphie);
 	setMotionMode(lastmode,true);
 
@@ -2327,10 +2331,7 @@ void TMC4671::estimateExtEnc(){
 	setPhiEtype(PhiE::ext);
 	setFluxTorque(0, 0);
 	setMotionMode(MotionMode::torque,true);
-	for(int16_t flux = 0; flux <= bangInitPower; flux+=1+bangInitPower/200){
-		setFluxTorque(flux, 0);
-		Delay(5);
-	}
+	rampFlux(bangInitPower, 1000);
 	int16_t phiE_enc = getPhiEfromExternalEncoder();
 	int16_t phiE_enc_old = 0;
 	int16_t rcount=0,c = 0; // Count how often direction was in reverse
@@ -2348,7 +2349,7 @@ void TMC4671::estimateExtEnc(){
 		}
 	}
 
-	setFluxTorque(0, 0);
+	rampFlux(0, 100);
 	setPhiEtype(lastphie);
 	setMotionMode(lastmode,true);