二段曲线改为过零处理

Signed-off-by: huhui <huhui@sharkgulf.com>

Applications/foc/core/e_ctrl.h

@@ -118,32 +118,28 @@ static void eRamp_set_step_target(e_Ramp *ramp, float c, u32 intval) {
 }
 
 static void eRamp_X2_running(e_Ramp *r) {
-#if 0
-	if (r->first_target != 0) {
-		r->interpolation += r->step_val;
-		if (r->step_val > 0) {
-			if (r->interpolation >= r->first_target) {
-				r->interpolation = r->first_target;
-				r->first_target = 0.0f;
-			}
-		}else {
-			if (r->interpolation <= r->first_target) {
-				r->interpolation = r->first_target;
-				r->first_target = 0.0f;
-			}
+#if 1
+	float target = r->target;
+	float v_now = r->interpolation;
+	bool cross_zero = false;
+	if (target > 0) {
+		if (v_now >= -CONFIG_RAMP_SECOND_TARGET && v_now <= CONFIG_RAMP_SECOND_TARGET * 1.5f) {
+			step_towards(&r->interpolation, target, 0.04f);
+			cross_zero = true;
+		}
+	}else if (target == 0) {
+		if (v_now >= 0 && v_now <= CONFIG_RAMP_SECOND_TARGET) {
+			step_towards(&r->interpolation, target, 0.01f);
+			cross_zero = true;
+		}
+	}else {
+		if (v_now >= -CONFIG_RAMP_SECOND_TARGET && v_now <= CONFIG_RAMP_SECOND_TARGET) {
+			step_towards(&r->interpolation, target, 0.01f);
+			cross_zero = true;
 		}
 	}
-	if (r->first_target != 0) {
-		return;
-	}
-	if(r->time < 0xFFFFFFFu) {
-		r->time ++;
-	}
-	r->interpolation = r->start + r->A * (float)SQ(r->time);
-	if ((r->A > 0) && (r->interpolation > r->target)) {
-		r->interpolation = r->target;
-	}else if ((r->A < 0) && (r->interpolation < r->target)) {
-		r->interpolation = r->target;
+	if (!cross_zero) {
+		step_towards(&r->interpolation, target, 1.0f);
 	}
 #else
 	if (r->first_step != 0) {
@@ -163,26 +159,9 @@ static void eRamp_X2_running(e_Ramp *r) {
 #endif
 }
 
-static void eRamp_set_X2_target(e_Ramp *ramp, float c) {
-#if 0
-	float c_now = eRamp_get_intepolation(ramp);
-
-	float delta = c - c_now;
-	float first_delta = 0;
-	if (delta > 0) {
-		first_delta = 0;//min(delta, 20.0f);
-		ramp->first_target = 0;//c_now + first_delta;
-		ramp->step_val = 0.05f;
-		ramp->A = (delta - first_delta)/SQ(ramp->acct);
-	}else {
-		first_delta = 0;//MAX(delta, -10.0f);
-		ramp->first_target = 0;//c_now + first_delta;
-		ramp->step_val = -0.01f;
-		ramp->A = (delta - first_delta)/SQ(ramp->dect);
-	}
-	ramp->start = c_now + first_delta;
-	ramp->time = 0;
-	eRamp_set_target(ramp, c);
+static void eRamp_set_X2_target(e_Ramp *r, float c) {
+#if 1
+	eRamp_set_target(r, c);
 #else
 	float c_now = eRamp_get_intepolation(ramp);
 	float step_val = 0;

Applications/foc/core/torque.c

@@ -171,7 +171,7 @@ void throttle_process(u8 run_mode, float f_throttle) {
 			torque_mode_process();
 		}
 	
-}else if (run_mode == CTRL_MODE_SPD) {
+	}else if (run_mode == CTRL_MODE_SPD) {
 		float spd_ref = throttle_to_speed(f_throttle);
 		g_trq_mn.spd_ref = LowPass_Filter(g_trq_mn.spd_ref, spd_ref, THRO_REF_LP_CEOF);
 		if ((g_trq_mn.count++ % 20) == 0) {

Applications/foc/foc_config.h

@@ -48,6 +48,7 @@
 #define CONFIG_RAMP_SECOND_STEP (0.1F)
 #define CONFIG_RAMP_SECOND_TARGET (5.0F)
 
+#define CONFIG_RAMP_CROSS_ZERO_STEP (0.1F)
 
 #define CURRENT_LOOP_RAMP_COUNT 300