Force fades up slowly on potential sharp position changes

Ultrawipf · Aug 23, 2024 · 7defddd · 7defddd
1 parent 88489b9
commit 7defddd
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 1 deletion.
diff --git a/Firmware/FFBoard/Inc/Axis.h b/Firmware/FFBoard/Inc/Axis.h
@@ -154,6 +154,8 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	int32_t getTorque(); // current torque scaled as a 32 bit signed value
 	int16_t updateEndstop();
 
+	void startForceFadeIn(float start = 0,float fadeTime = 0.5);
+
 	metric_t* getMetrics();
 
 	void setEffectTorque(int32_t torque);
@@ -185,6 +187,9 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 
 	const Error outOfBoundsError = Error(ErrorCode::axisOutOfRange,ErrorType::warning,"Axis out of bounds");
 
+	float forceFadeTime = 1.0;
+	float forceFadeCurMult = 1.0;
+
 #ifdef TMC4671DRIVER
 	TMC4671Limits tmclimits = TMC4671Limits({.pid_torque_flux_ddt = 32767,
 											 .pid_uq_ud = 30000,
@@ -253,6 +258,7 @@ class Axis : public PersistentStorage, public CommandHandler, public ErrorHandle
 	int16_t idlespringclip = 0;
 	float idlespringscale = 0;
 	bool idle_center = false;
+	bool motorWasNotReady = true;
 
 	// TODO tune these and check if it is really stable and beneficial to the FFB. index 4 placeholder
 	const biquad_constant_t filterSpeedCst[4] = {{ 30, 55 }, { 60, 55 }, { 120, 55 }, {120, 55}};

diff --git a/Firmware/FFBoard/Src/Axis.cpp b/Firmware/FFBoard/Src/Axis.cpp
@@ -261,6 +261,7 @@ uint8_t Axis::getEncType(){
 
 void Axis::setPos(uint16_t val)
 {
+	startForceFadeIn(0.25,0.5);
 	if(this->drv != nullptr){
 		drv->getEncoder()->setPos(val);
 	}
@@ -318,6 +319,13 @@ void Axis::prepareForUpdate(){
 		//ErrorHandler::clearError(outOfBoundsError);
 	}
 
+	// On first change to ready start a fade
+	if(motorWasNotReady && drv->motorReady()){
+		motorWasNotReady = false;
+		startForceFadeIn(0, 1.0);
+	}
+
+
 	this->updateMetrics(angle);
 
 }
@@ -487,8 +495,10 @@ float Axis::getEncAngle(Encoder *enc){
  */
 void Axis::emergencyStop(bool reset){
 	drv->turn(0); // Send 0 torque first
+	if(reset){
+		startForceFadeIn();
+	}
 	drv->emergencyStop(reset);
-	//drv->stopMotor();
 	control->emergency = !reset;
 }
 
@@ -506,6 +516,7 @@ void Axis::usbSuspend(){
  * Enables motor driver
  */
 void Axis::usbResume(){
+	startForceFadeIn();
 	if (drv != nullptr){
 		drv->startMotor();
 	}
@@ -725,6 +736,12 @@ bool Axis::updateTorque(int32_t* totalTorque) {
 		torque = 0;
 	}
 
+	// Fade in
+	if(forceFadeCurMult < 1){
+		torque = torque * forceFadeCurMult;
+		forceFadeCurMult += forceFadeTime / this->filter_f; // Fade time
+	}
+
 	// Torque calculated. Now sending to driver
 	torque = (invertAxis) ? -torque : torque;
 	metric.current.torque = torque;
@@ -740,6 +757,15 @@ bool Axis::updateTorque(int32_t* totalTorque) {
 	return (torqueChanged);
 }
 
+/**
+ * Starts fading in force from start to 1 over fadeTime
+ */
+void Axis::startForceFadeIn(float start,float fadeTime){
+	this->forceFadeTime = fadeTime;
+	this->forceFadeCurMult = clip<float>(start, 0, 1);
+}
+
+
 /**
  * Changes gamepad range in degrees for effect scaling
  */