knob: Tweak for INERTIA mode

xingrz · Jan 17, 2023 · ae2a690 · ae2a690
1 parent e1a44af
commit ae2a690
Showing 1 changed file with 26 additions and 7 deletions.
diff --git a/config/drivers/sensor/knob/knob.c b/config/drivers/sensor/knob/knob.c
@@ -88,14 +88,33 @@ static void knob_tick(const struct device *dev)
 	switch (data->mode) {
 	case KNOB_INERTIA: {
 		float v = knob_get_velocity(dev);
-		if (v > 1 || v < -1) {
-			if (fabsf(v - data->last_velocity) > 0.3) {
+		float a = v - data->last_velocity;
+		if (v == 0.0f) {
+			mc->target = 0.0f;
+		} else if (v > 0.0f) {
+			if (a > 1.0f) {
 				mc->target = v;
+			} else if (a < -2.0f) {
+				mc->target += a;
+				if (mc->target < 1.0f) {
+					mc->target = 0.0f;
+				}
+			} else {
+				mc->target -= 0.001f;
+			}
+		} else if (v < 0.0f) {
+			if (a < -1.0f) {
+				mc->target = v;
+			} else if (a > 2.0f) {
+				mc->target += a;
+				if (mc->target > -1.0f) {
+					mc->target = 0.0f;
+				}
+			} else {
+				mc->target += 0.001f;
 			}
-		} else {
-			mc->target = 0.0f;
 		}
-		data->last_velocity = v;
+		data->last_velocity = mc->target;
 	} break;
 	case KNOB_ENCODER: {
 		float dp = knob_get_position(dev) - data->last_angle;
@@ -168,9 +187,9 @@ void knob_set_mode(const struct device *dev, enum knob_mode mode)
 		break;
 	case KNOB_INERTIA: {
 		motor_set_enable(config->motor, true);
-		motor_set_torque_limit(config->motor, 0.5f);
+		motor_set_torque_limit(config->motor, 1.5f);
 		mc->mode = VELOCITY;
-		motor_set_velocity_pid(config->motor, 0.1f, 0.0f, 0.0f);
+		motor_set_velocity_pid(config->motor, 0.3f, 0.0f, 0.0f);
 		motor_set_angle_pid(config->motor, 20.0f, 0.0f, 0.7f);
 		mc->target = 0.0f;
 	} break;