同心校准，通过matlab设计FIR滤波器，采样率500HZ,通过频率0.5HZ,截至频率5Hz

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

Applications/app/app.c

@@ -4,6 +4,7 @@
 #include "libs/logger.h"
 #include "libs/utils.h"
 #include "foc/motor/motor.h"
+#include "foc/motor/current.h"
 #include "foc/samples.h"
 #include "prot/can_foc_msg.h"
 #include "prot/can_message.h"
@@ -21,32 +22,39 @@ extern measure_time_t g_meas_timeup;
 #ifdef JTAG_DEBUG
 int jtag_cmd = 0;
 int jtag_data = 0;
+int jtag_plot = 0;
 void fetch_jtag_cmd(void) {
 	foc_cmd_body_t foc_cmd;
 	u8 cmd_data[32];
 	if (jtag_cmd == 1 || jtag_cmd == 2) {
+		jtag_plot = 2;
 		foc_cmd.cmd = Foc_Start_Motor;
 		foc_cmd.data = cmd_data;
 		cmd_data[0] = jtag_cmd;
 		foc_send_command(&foc_cmd);
 		jtag_cmd = 0;
 	}else if (jtag_cmd == 3) {
+		
 		float vq = (float)jtag_data/10.0f;
 		PMSM_FOC_SetOpenVdq(0, (vq));
 		jtag_cmd = 0;
 	}else if (jtag_cmd == 4) {
+		jtag_plot = 1;
 		foc_cmd.cmd = Foc_Cali_Hall_Phase;
 		foc_cmd.data = cmd_data;
 		encode_s16(cmd_data, jtag_data);
 		foc_send_command(&foc_cmd);
 		jtag_cmd = 0;
 	}else if (jtag_cmd == 5) {
-		PMSM_FOC_Set_Current(0.5f);
+		PMSM_FOC_Set_Current((float)jtag_data/10.0f);
 		jtag_cmd = 0;
 	}else if (jtag_cmd == 6) {
 		PMSM_FOC_EnableCruise(true);
 	}else if (jtag_cmd == 7) {
 		PMSM_FOC_EnableCruise(false);
+	}else if (jtag_cmd == 8) {
+		//mc_current_sensor_calibrate((float)jtag_data/10.0f);
+		jtag_cmd = 0;
 	}
 }
 #else

Applications/app/nv_storage.c

@@ -31,8 +31,8 @@ static void nv_default_motor_params(void) {
 	m_params.r = MOTOR_R;
 	m_params.ld = MOTOR_Ld;
 	m_params.lq = MOTOR_Lq;
-	m_params.encoder_offset = -50;
-	m_params.est_pll_band = 20;
+	m_params.encoder_offset = (360-128);
+	m_params.est_pll_band = 200;
 	m_params.pos_lock_pll_band = 200;
 	m_params.flux_linkage = 0.0f;
 	m_params.hall_offset = 0;
@@ -41,11 +41,11 @@ static void nv_default_motor_params(void) {
 static void nv_default_foc_params(void) {
 	foc_params.s_maxvDC = 48;
 	foc_params.s_maxiDC = 30;
-	foc_params.s_maxIdq = 150;
-	foc_params.s_minIdq = -150;
+	foc_params.s_maxIdq = 30;
+	foc_params.s_minIdq = -30;
 	foc_params.s_maxRPM = 8200;
 	foc_params.s_maxEpmRPM = 133;
-	foc_params.s_maxTorque = 100;
+	foc_params.s_maxTorque = 30;
 	foc_params.s_maxBrkCurrent = 2.0f;
 	foc_params.n_currentBand = 500;
 	foc_params.n_modulation = 1.0f;
@@ -109,5 +109,7 @@ void nv_read_foc_params(void) {
 void nv_storage_init(void) {
 	nv_read_motor_params();
 	nv_read_foc_params();
+	nv_default_motor_params();
+	nv_default_foc_params();
 }
 

Applications/bsp/adc.c

@@ -64,6 +64,10 @@ static void adc0_init(void){
 #endif
 #ifdef W_PHASE_I_CHAN	
 	adc_update_insert_sample_time(ADC0, W_PHASE_I_CHAN, ADC_SAMPLE_TIME);
+#endif
+#ifdef HIGH_SIDE_CURRENT_SENSOR
+	adc_oversample_mode_config(ADC0, ADC_OVERSAMPLING_ALL_CONVERT, ADC_OVERSAMPLING_SHIFT_3B, ADC_OVERSAMPLING_RATIO_MUL8);
+	adc_oversample_mode_enable(ADC0);
 #endif
     /* configure ADC inserted channel trigger */
     adc_external_trigger_source_config(ADC0, ADC_INSERTED_CHANNEL, ADC_TRIGGER_PHASE);
@@ -117,6 +121,10 @@ static void adc1_init(void){
 #endif
 #ifdef W_PHASE_I_CHAN	
 	adc_update_insert_sample_time(ADC1, W_PHASE_I_CHAN, ADC_SAMPLE_TIME);
+#endif
+#ifdef HIGH_SIDE_CURRENT_SENSOR
+	adc_oversample_mode_config(ADC1, ADC_OVERSAMPLING_ALL_CONVERT, ADC_OVERSAMPLING_SHIFT_3B, ADC_OVERSAMPLING_RATIO_MUL8);
+	adc_oversample_mode_enable(ADC1);
 #endif
     /* ADC external trigger enable */
 	adc_external_trigger_source_config(ADC1, ADC_INSERTED_CHANNEL, ADC_TRIGGER_NONE);

Applications/bsp/board_gd32demo.h

@@ -29,6 +29,7 @@
 
 /* MOS 驱动 */
 #define pwm_timer TIMER0
+#define PWM_MODE TIMER_OC_MODE_PWM0
 #define PWM_U_P_GROUP 	GPIOA
 #define PWM_U_P_PIN 	GPIO_PIN_8
 #define PWM_U_P_RCU 	RCU_GPIOA
@@ -145,6 +146,26 @@
 #define ENC_PWM_TIMER_INT_FLG TIMER_INT_FLAG_CH0
 #define ENC_PWM_IRQHandler TIMER3_IRQHandler
 
+#define ENC_MAX_interpolation 1.0f
+#define ENC_FILTER_NR          4
+
+/* 编码器参数      */
+#define ENC_MAX_RES  4096
+/*min. 490 Hz, max 603 Hz*/
+#define ENC_PWM_Max_P (1.0f/490.0f)
+#define ENC_PWM_Min_P (1.0f/603.0f)
+
+#define ENC_PWM_MAX_RES 4119.0F
+#define ENC_PWM_INIT_WIDTH 12.0F //PWM 起始宽度
+#define ENC_PWM_ERROR_WIDTH 4.0f //PWM 指示错误的宽度
+#define ENC_PWM_END_WIDTH   0.0F
+#define ENC_PWM_Error_P (ENC_PWM_INIT_WIDTH/ENC_PWM_MAX_RES)
+#define ENC_PWM_MIN_duty ((ENC_PWM_INIT_WIDTH + ENC_PWM_ERROR_WIDTH + ENC_PWM_END_WIDTH)/ENC_PWM_MAX_RES)
+
+#define ENC_Duty_2_Pluse_Nr(duty) (duty * ENC_PWM_MAX_RES - (ENC_PWM_INIT_WIDTH + ENC_PWM_ERROR_WIDTH + ENC_PWM_END_WIDTH)) //通过占空比计算有几个脉冲
+#define ENC_Pluse_Nr_2_angle(Nr) (360.0f/(float)ENC_MAX_RES * (Nr))
+
+#define ENC_Duty(d, t) ((d)/(t))
 
 #endif /*_BOARD_GD32DEMO_H__ */
 

Applications/bsp/board_mc_v1.h

@@ -11,7 +11,7 @@
 #define SCHED_TIMER_IRQ TIMER5_IRQn
 #define SCHED_TIMER_IRQHandler TIMER5_IRQHandler
 
-#define PWM_DEAD_TIME_NS 300u
+#define PWM_DEAD_TIME_NS 200u
 #define HW_DEAD_TIME_NS  200u
 #define HW_RISE_TIME_NS  500u
 #define HW_NOISE_TIME_NS 300u
@@ -19,7 +19,7 @@
 #define TDead NS_2_TCLK(HW_DEAD_TIME_NS + PWM_DEAD_TIME_NS)/* ����ʱ�� */ 
 #define TRise NS_2_TCLK(HW_RISE_TIME_NS)/* MOS ����ʱ��*/
 #define TNoise NS_2_TCLK(HW_NOISE_TIME_NS)/* MOS��������Ŀ�������ʱ�� */
-#define TADC  ((uint16_t)((ADC_TRIG_CONV_LATENCY_CYCLES + ADC_SAMPLING_CYCLES) * TIM_CLOCK_MHz) / ADC_CLOCK_MHz + 1u)/* ADC ����ʱ�� */
+#define TADC  ((uint16_t)((ADC_TRIG_CONV_LATENCY_CYCLES + ADC_SAMPLING_CYCLES*8) * TIM_CLOCK_MHz) / ADC_CLOCK_MHz + 1u)/* ADC ����ʱ�� */
 #define TSampleMIN (TDead + TRise + TADC) //采样需要的总时间
 #define TSampleBefore (TDead + TRise) //采样开始前需要等待的时间
 
@@ -27,6 +27,7 @@
 
 /* MOS驱动 */
 #define pwm_timer TIMER0
+#define PWM_MODE TIMER_OC_MODE_PWM1
 #define PWM_U_P_GROUP 	GPIOA
 #define PWM_U_P_PIN 	GPIO_PIN_8
 #define PWM_U_P_RCU 	RCU_GPIOA
@@ -60,7 +61,7 @@
 #define PWM_BRAKE_GROUP 	GPIOB
 #define PWM_BRAKE_PIN 	GPIO_PIN_12
 #define PWM_BRAKE_RCU 	RCU_GPIOB
-#define PWM_BRAKE_MODE 	GPIO_MODE_IPU
+#define PWM_BRAKE_MODE 	GPIO_MODE_IN_FLOATING
 
 
 /* 高边电流传感器采样 */
@@ -69,6 +70,8 @@
 #define V_PHASE_I_CHAN  ADC_CHANNEL_5
 #define W_PHASE_I_CHAN  ADC_CHANNEL_6
 
+#define PWM_UV_SWAP
+
 #define V_PHASE_ADC_GROUP 	GPIOA
 #define V_PHASE_ADC_PIN 	GPIO_PIN_5
 #define V_PHASE_ADC_RCU 	RCU_GPIOA
@@ -89,6 +92,9 @@
 #define VBUS_V_ADC_RCU 		RCU_GPIOA
 #define VBUS_V_ADC_MODE 	GPIO_MODE_AIN
 
+/* 是否有目前电流采集 */
+#define NO_SAMPLE_IDC //如果硬件没有采集母线电流，定义一下
+
 /*
 #define THROTTLE_CHAN ADC_CHANNEL_1 //转把信号
 #define THROTTLE_V_ADC_GROUP 	GPIOA
@@ -97,12 +103,14 @@
 #define THROTTLE_V_ADC_MODE 	GPIO_MODE_AIN
 */
 
-#define ADC_TO_CURR_ceof (0.37f)
+#define ADC_TO_CURR_ceof1 (0.3362f)
+#define ADC_TO_CURR_ceof2 (0.3404f)
+
 #define VBUS_VOL_CEOF (ADC_REFERENCE_VOLTAGE*45/4096.0f)
 #define THROTTLE_VOL_CEOF (1)
 
 /* 刹车手把输入 */
-#define GPIO_BRAKE_IN_GROUP 	GPIOA
+//#define GPIO_BRAKE_IN_GROUP 	GPIOA
 #define GPIO_BRAKE_IN_PIN 	GPIO_PIN_2
 #define GPIO_BRAKE_IN_RCU 	RCU_GPIOA
 #define GPIO_BRAKE_IN_MODE 	GPIO_MODE_IN_FLOATING
@@ -132,6 +140,7 @@
 #define ENC_PWM_PIN GPIO_PIN_15
 #define ENC_PWM_RCU RCU_GPIOA
 #define ENC_PWM_MODE GPIO_MODE_IN_FLOATING
+#define TIMER1_PA15_REMAP GPIO_TIMER1_PARTIAL_REMAP0
 
 #define ENC_I_GROUP GPIOB     /*测量编码器的ABI的I信号，360度同步一次*/
 #define ENC_I_PIN GPIO_PIN_8
@@ -155,5 +164,21 @@
 #define ENC_PWM_TIMER_INT_FLG TIMER_INT_FLAG_CH0
 #define ENC_PWM_IRQHandler TIMER1_IRQHandler
 
+#define ENC_MAX_interpolation 4.0F
+
+#define ENC_FILTER_NR          20
+
+/* 编码器参数      */
+#define ENC_MAX_RES  1024
+#define ENC_PWM_Min_P (1.0f/131.0f)
+
+#define ENC_Duty_2_Pluse_Nr(duty) (duty * ENC_MAX_RES) //通过占空比计算有几个脉冲
+#define ENC_Pluse_Nr_2_angle(Nr) (360.0f/(float)ENC_MAX_RES * (Nr))
+
+#define ENC_Duty(d, t) ((1.0f/128.0f) * (130.0f * (d)/(t) - 1.0f))
+
+
+#define DEBUG_PORT_UART1
+
 #endif /*_BOARD_MC_V1_H__ */
 

Applications/bsp/bsp.h

@@ -21,7 +21,7 @@
 #define ADC_CLOCK_MHz (30u)
 #define NS_PER_TCLK (8u) /* (1/120000000 * 1000000000) */
 #define NS_2_TCLK(ns) (((ns)/NS_PER_TCLK) + 1u) //ns תΪpwmʹ�õ��Ǹ�TIM��clk count
-#define FOC_PWM_FS (20000u)
+#define FOC_PWM_FS (16000u)
 #define FOC_PWM_period (TIM_CLOCK/FOC_PWM_FS)
 #define FOC_PWM_Half_Period (FOC_PWM_period/2)
 

Applications/bsp/enc_intf.c

@@ -8,6 +8,9 @@ static void _io_init(void) {
 	rcu_periph_clock_enable(ENC_I_RCU);
 #ifdef TIMER2_PB4_PB5_REMAP
 	gpio_pin_remap_config(TIMER2_PB4_PB5_REMAP, ENABLE);
+#endif
+#ifdef TIMER1_PA15_REMAP
+	gpio_pin_remap_config(TIMER1_PA15_REMAP, ENABLE);
 #endif
 	gpio_init(ENC_A_GROUP, ENC_A_MODE, GPIO_OSPEED_50MHZ, ENC_A_PIN);
 	gpio_init(ENC_B_GROUP, ENC_B_MODE, GPIO_OSPEED_50MHZ, ENC_B_PIN);
@@ -57,7 +60,7 @@ void enc_intf_quadrature_init(u32 rate) {
     timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
     timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI;
     timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1;
-    timer_icinitpara.icfilter    = 0x4;//采样频率120M, 这个必须要加入滤波，否则AB的计数可能不对
+    timer_icinitpara.icfilter    = ENC_FILTER_NR;//采样频率120M, 这个必须要加入滤波，否则AB的计数可能不对
     timer_input_capture_config(timer,TIMER_CH_0,&timer_icinitpara);
 	
 	timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
@@ -102,7 +105,7 @@ void enc_intf_pwm_counter(void) {
 	timer_icinitpara.icpolarity  = TIMER_IC_POLARITY_RISING;
 	timer_icinitpara.icselection = TIMER_IC_SELECTION_DIRECTTI;
 	timer_icinitpara.icprescaler = TIMER_IC_PSC_DIV1;
-	timer_icinitpara.icfilter	 = 0x0;
+	timer_icinitpara.icfilter	 = ENC_FILTER_NR;
 	timer_input_pwm_capture_config(timer, ENC_PWM_TIMER_CHAN, &timer_icinitpara);
 
 	/* slave mode selection: TIMER */

Applications/bsp/pwm.c

@@ -104,17 +104,17 @@ static void _init_pwm_timer(void) {
 
     timer_channel_output_config(timer,TIMER_CH_0,&timer_ocintpara);
     timer_channel_output_pulse_value_config(timer,TIMER_CH_0,half_period/2);
-    timer_channel_output_mode_config(timer,TIMER_CH_0,TIMER_OC_MODE_PWM1);
+    timer_channel_output_mode_config(timer,TIMER_CH_0,PWM_MODE);
     timer_channel_output_shadow_config(timer,TIMER_CH_0,TIMER_OC_SHADOW_ENABLE);
 
     timer_channel_output_config(timer,TIMER_CH_1,&timer_ocintpara);
     timer_channel_output_pulse_value_config(timer,TIMER_CH_1,half_period/2);
-    timer_channel_output_mode_config(timer,TIMER_CH_1,TIMER_OC_MODE_PWM1);
+    timer_channel_output_mode_config(timer,TIMER_CH_1,PWM_MODE);
     timer_channel_output_shadow_config(timer,TIMER_CH_1,TIMER_OC_SHADOW_ENABLE);
 
     timer_channel_output_config(timer,TIMER_CH_2,&timer_ocintpara);
     timer_channel_output_pulse_value_config(timer,TIMER_CH_2,half_period/2);
-    timer_channel_output_mode_config(timer,TIMER_CH_2,TIMER_OC_MODE_PWM1);
+    timer_channel_output_mode_config(timer,TIMER_CH_2,PWM_MODE);
     timer_channel_output_shadow_config(timer,TIMER_CH_2,TIMER_OC_SHADOW_ENABLE);
     
     timer_ocintpara.outputstate  = TIMER_CCX_ENABLE;
@@ -136,7 +136,7 @@ static void _init_pwm_timer(void) {
     timer_breakpara.ideloffstate       = TIMER_ROS_STATE_DISABLE;
     timer_breakpara.protectmode        = TIMER_CCHP_PROT_OFF; 
     timer_breakpara.deadtime           = NS_2_TCLK(PWM_DEAD_TIME_NS);
-    timer_breakpara.breakstate         = TIMER_BREAK_ENABLE;
+    timer_breakpara.breakstate         = TIMER_BREAK_DISABLE;
     timer_breakpara.breakpolarity      = TIMER_BREAK_POLARITY_LOW;
     timer_breakpara.outputautostate    = TIMER_OUTAUTO_DISABLE;
     timer_break_config(timer,&timer_breakpara);

Applications/bsp/pwm.h

@@ -17,6 +17,7 @@
 #define ch2_update_duty(duty) 	TIMER_CH2CV(pwm_timer) = (uint32_t)duty
 #define update_adc_trigger(time) TIMER_CH3CV(pwm_timer) = (uint32_t)time
 
+#ifdef PWM_UV_SWAP
 #define pwm_update_duty(dutyA, dutyB, dutyC) \
 	do {\
 		ch0_update_duty(dutyC);\
@@ -24,6 +25,16 @@
 		ch2_update_duty(dutyA);\
 	}while(0)
 
+#else
+#define pwm_update_duty(dutyA, dutyB, dutyC) \
+	do {\
+		ch0_update_duty(dutyA);\
+		ch1_update_duty(dutyB);\
+		ch2_update_duty(dutyC);\
+	}while(0)
+
+#endif
+
 #define pwm_update_2smaples(samp1, sampl2) \
 	do { \
 		TIMER_CH3CV(pwm_timer) = (uint32_t)samp1; \

Applications/bsp/uart.c

@@ -4,9 +4,9 @@
 #include "libs/logger.h"
 #include "libs/utils.h"
 
-#define SHARK_UART_BAUDRATE				921600
+#define SHARK_UART_BAUDRATE				500000
 
-#ifdef GD32_FOC_DEMO
+#ifdef DEBUG_PORT_UART1
 #define SHARK_UART0_com					USART1
 #define SHARK_UART0_tx_port				GPIOA
 #define SHARK_UART0_tx_pin				GPIO_PIN_2
@@ -75,10 +75,10 @@ static bool shark_uart_on_rx_frame(shark_uart_t *uart)
 
 static void shark_uart_rx(shark_uart_t *uart){
 	while(1) {
-		u8 data;
+		u8 data = 0;
 		update_dma_w_pos(uart);
 		if (circle_get_one_data(&uart->rx_queue, &data) != 1) {
-			break;
+			return;
 		}
 		switch(data){
 			case CH_START:

Applications/foc/core/PMSM_FOC_Core.c

@@ -188,7 +188,8 @@ static __INLINE void PMSM_FOC_Update_PI_Idq(void) {
 		_gFOC_Ctrl.pi_ctl_iq->min = _gFOC_Ctrl.params.minvDQ.q;
 	}	
 }
-
+extern int jtag_plot;
+float encoder_last_offset(void);
 static __INLINE void PMSM_FOC_Plot_Debug(void) {
 	if (_gFOC_Ctrl.ctrl_count % 8 == 0) {
 		//plot_1data16(_gFOC_Ctrl.out.test_sample);
@@ -199,9 +200,11 @@ static __INLINE void PMSM_FOC_Plot_Debug(void) {
 		//plot_2data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle), FtoS16(_gFOC_Ctrl.in.s_motAngle));
 		//plot_3data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.out.s_OutVdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_OutVdq.q));
 		//plot_3data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.q));
-		//plot_3data16(FtoS16x1000(iabc[0]), FtoS16x1000(iabc[1]), _gFOC_Ctrl.out.test_sample*100);
-		//plot_2data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle), FtoS16(encoder_get_pwm_angle()));
-		plot_1data16(_gFOC_Ctrl.in.s_motRPM);
+		if (jtag_plot == 2) {
+			plot_3data16(FtoS16(_gFOC_Ctrl.in.s_iABC[1]), FtoS16(_gFOC_Ctrl.in.s_motRPM), FtoS16(_gFOC_Ctrl.in.s_hallAngle));
+		}
+		//plot_1data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle));
+		//plot_1data16(_gFOC_Ctrl.in.s_motRPM);
 	}
 }
 
@@ -235,7 +238,7 @@ void PMSM_FOC_Schedule(void) {
 
 	}else {
 		_gFOC_Ctrl.in.s_targetVdq.d = FOC_Get_DqRamp(&_gFOC_Ctrl.vdq_ctl[0]);
-		_gFOC_Ctrl.in.s_targetVdq.q = FOC_Get_DqRamp(&_gFOC_Ctrl.vdq_ctl[1]);
+		_gFOC_Ctrl.in.s_targetVdq.q = FOC_Get_DqRamp(&_gFOC_Ctrl.vdq_ctl[1]);	
 	}
 	_gFOC_Ctrl.out.f_vdqRation = Circle_Limitation(&_gFOC_Ctrl.in.s_targetVdq, _gFOC_Ctrl.in.s_vDC, _gFOC_Ctrl.params.n_modulation, &_gFOC_Ctrl.out.s_OutVdq);
 	
@@ -244,9 +247,6 @@ void PMSM_FOC_Schedule(void) {
 	SVM_Duty_Fix(&vAB, _gFOC_Ctrl.in.s_vDC, FOC_PWM_Half_Period, &_gFOC_Ctrl.out);
 
 	phase_current_point(&_gFOC_Ctrl.out);
-	_gFOC_Ctrl.out.n_Duty[0] = FOC_PWM_Half_Period-NS_2_TCLK(PWM_DEAD_TIME_NS)*2;
-	_gFOC_Ctrl.out.n_Duty[1] = NS_2_TCLK(PWM_DEAD_TIME_NS)*2;
-	_gFOC_Ctrl.out.n_Duty[2] = FOC_PWM_Half_Period-NS_2_TCLK(PWM_DEAD_TIME_NS)*2;
 	
 	pwm_update_duty(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
 	pwm_update_sample(_gFOC_Ctrl.out.n_Sample1, _gFOC_Ctrl.out.n_Sample2, _gFOC_Ctrl.out.n_CPhases);

Applications/foc/motor/current.c

@@ -64,7 +64,7 @@ void phase_current_sensor_start_calibrate(float calibrate_current) {
 		calibrate = true;
 		g_cs.sensor_samples_1 = 0;
 		g_cs.sensor_samples_2 = 0;
-		g_cs.sample_count = SENSOR_SAMPLES;
+		g_cs.sample_count = SENSOR_SAMPLES + 1;
 		g_cs.calibrate_current = calibrate_current;
 	}
 	
@@ -81,13 +81,17 @@ bool phase_current_sensor_do_calibrate(void) {
 	if(cs->c_phases == PHASE_BC) {
 		if (cs->sample_count > 0) {
 			cs->sample_count--;
-			cs->sensor_samples_1 += (phase_current1 - cs->adc_offset_b);
-			cs->sensor_samples_2 += (phase_current2 - cs->adc_offset_c);
+			if (cs->sample_count <= SENSOR_SAMPLES) {
+				cs->sensor_samples_1 += (phase_current1 - cs->adc_offset_b);
+				cs->sensor_samples_2 += (phase_current2 - cs->adc_offset_c);
+			}
 		}else {
 			cs->sensor_samples_1 = cs->sensor_samples_1 / (float)SENSOR_SAMPLES;
 			cs->sensor_samples_2 = cs->sensor_samples_2 / (float)SENSOR_SAMPLES;
 			cs->sensor_k1 = g_cs.calibrate_current/cs->sensor_samples_1;
 			cs->sensor_k2 = g_cs.calibrate_current/cs->sensor_samples_2;
+			cs->sensor_k1 = ABS(cs->sensor_k1);
+			cs->sensor_k2 = ABS(cs->sensor_k2);
 			cs->is_calibrating_sensor = false;
 		}
 	}
@@ -282,7 +286,6 @@ void phase_current_get(float *iABC){
 			cs->adc_ib = cs->adc_ib_filter;
 			cs->adc_ic = cs->adc_ic_filter;
 		}
-
 		cs->adc_ia = -(cs->adc_ib + cs->adc_ic);
 	}else if (cs->c_phases == PHASE_AC) {
 		/* Current on Phase B is not accessible 	*/
@@ -307,9 +310,15 @@ void phase_current_get(float *iABC){
 		}
 		cs->adc_ib = -(cs->adc_ia + cs->adc_ic);
 	}
+#ifdef HIGH_SIDE_CURRENT_SENSOR
+	iABC[1] = -cs->adc_ib * ADC_TO_CURR_ceof1;
+	iABC[2] = -cs->adc_ic * ADC_TO_CURR_ceof2;
+	iABC[0] = -(iABC[1] + iABC[2]);
+#else
 	iABC[0] = -cs->adc_ia * ADC_TO_CURR_ceof;
 	iABC[1] = -cs->adc_ib * ADC_TO_CURR_ceof;
 	iABC[2] = -cs->adc_ic * ADC_TO_CURR_ceof;
+#endif
 }
 
 void phase_current_point(void *p){
@@ -323,7 +332,7 @@ void phase_current_point(void *p){
 	cs->c_ignore_phase = IGNORE_NONE;
 #ifdef HIGH_SIDE_CURRENT_SENSOR
 	cs->c_phases = PHASE_BC;
-	out->n_Sample1 = FOC_PWM_Half_Period/4;
+	out->n_Sample1 = FOC_PWM_Half_Period - TADC/2;
 #else
 	u16 *duty = out->n_Duty;
 	u16 max_hi_side = MAX(duty[0], MAX(duty[1], duty[2]));

Applications/foc/motor/encoder.c

@@ -5,15 +5,26 @@
 #include "foc/motor/motor_param.h"
 #include "libs/logger.h"
 #include "app/nv_storage.h"
+#include "math/fast_math.h"
+
+#include "encoder_off.h"
 
 #define ANGLE_OFFSET (-50.0f)//133.0f
 /* 磁编码器使用一对极的磁铁，所以编码器获取的角度和机械角度相同需要转为电角度*/
 #define DIR_ADJUGE_MAX_CNT 10
 #define PLL_BANDWIDTH 200
-static encoder_t g_encoder;
+encoder_t g_encoder;
 
-#define USE_PWM_ONLY
+u8   encoder_off_count[1024] = {0};
+s16  encoder_off_map[1024] = {0};
+s16  encoder_off_r_map[1024] = {0};
+u8   encoder_off_r_count[1024] = {0};
 
+bool encoder_off_finished = false;
+s16  encoder_off_comp = 940.0f;
+float encoder_off_mul = 1.0f;
+int encoder_off_cn_add = 542;
+float encoder_off_angle_add = 0.0f;
 static __INLINE void encoder_pll_update_gain(void) {
 	if (g_encoder.pll_bandwidth_shadow != g_encoder.pll_bandwidth) {
 		g_encoder.pll_bandwidth = g_encoder.pll_bandwidth_shadow;
@@ -33,6 +44,12 @@ static void _init_pll(void) {
 
 
 void encoder_init(void) {
+	for (int i = i; i < 1024; i++) {
+		encoder_off_count[i] = 0;
+		encoder_off_map[i] = 0;
+		encoder_off_r_map[i] = 0;
+		encoder_off_r_count[i] = 0;
+	}
 	encoder_init_clear(POSITIVE);
 	enc_intf_init(ENC_MAX_RES);
 }
@@ -111,20 +128,34 @@ float encoder_get_theta(void) {
 		g_encoder.b_timer_ov = true;
 		ENC_ClearUpFlags();
 	}
-	if (cnt == g_encoder.last_cnt) {
-		g_encoder.interpolation += g_encoder.est_vel_counts * FOC_CTRL_US;
-		if (g_encoder.interpolation > 1.0f) {
-			g_encoder.interpolation = 1.0f;
-		}else if (g_encoder.interpolation < -1.0f) {
-			g_encoder.interpolation = -1.0f;
-		}
+	encoder_run_pll((float)(cnt));
+    bool snap_to_zero_vel = false;
+    if (ABS(g_encoder.est_pll.out) < 0.5f * g_encoder.est_pll.DT * g_encoder.est_pll.ki) {
+        g_encoder.est_vel_counts = g_encoder.est_pll.out = 0.0f;  // align delta-sigma on zero to prevent jitter
+        snap_to_zero_vel = true;
+    }
+	if (snap_to_zero_vel) {
+		g_encoder.interpolation = 0.5f;
 	}else {
-		g_encoder.interpolation = 0.0f;
+		if (cnt == g_encoder.last_cnt) {
+			g_encoder.interpolation += g_encoder.est_vel_counts * FOC_CTRL_US;
+			if (g_encoder.interpolation > ENC_MAX_interpolation) {
+				g_encoder.interpolation = ENC_MAX_interpolation;
+			}else if (g_encoder.interpolation < -ENC_MAX_interpolation) {
+				g_encoder.interpolation = -ENC_MAX_interpolation;
+			}
+		}else {
+			g_encoder.interpolation = 0.0f;
+		}
 	}
 	g_encoder.abi_angle = ENC_Pluse_Nr_2_angle((float)cnt + g_encoder.interpolation) * g_encoder.motor_poles + g_encoder.enc_offset;
+	if (encoder_off_finished) {
+		int cnt_off = (cnt + FIR_PHASE_SHIFT) % g_encoder.cpr;
+		g_encoder.abi_angle -= ((_encoder_off_map[cnt_off])/100.0f);
+	}
 	rand_angle(g_encoder.abi_angle);
 
-	encoder_run_pll((float)(cnt));
+	
 	g_encoder.last_cnt = cnt;
 	g_encoder.last_us = timer_count32_get();
 	return g_encoder.abi_angle;
@@ -134,8 +165,69 @@ float encoder_get_speed(void) {
 	return (g_encoder.est_vel_counts/g_encoder.cpr) * 60.0f * g_encoder.motor_poles;
 }
 
-void encoder_detect_offset(float angle){
-	//plot_3data16(angle, g_encoder.pwm_angle, g_encoder.abi_angle);
+
+extern int jtag_plot;
+void encoder_detect_offset(float angle, bool r){
+	float delta = (g_encoder.abi_angle - angle);
+	if (delta > 200) {
+		delta = delta - 360;
+	}
+	if (delta < -200) {
+		delta = delta + 360;
+	}
+	int last_cn = (g_encoder.last_cnt + 0) % 1024;
+	if (!r) {
+		encoder_off_map[last_cn] = (s16)(delta*100.0f);
+		encoder_off_count[last_cn] ++;
+	}else {
+		encoder_off_r_map[last_cn] = (s16)(delta*100.0f);
+		encoder_off_r_count[last_cn] ++;
+	}
+	if (jtag_plot==1) {
+		plot_1data16((s16)(delta*100.0f));
+	}
+	encoder_off_finished = false;
+}
+
+
+float encoder_detect_finish(bool r) {
+	for (int i = 0; i < 1024; i++) {
+		if (!r) {
+			if (encoder_off_count[i] <= 2) {
+				return false;
+			}
+		}else {
+			if (encoder_off_r_count[i] <= 2) {
+				return false;
+			}
+		}
+	}
+	encoder_off_finished = true;
+	return encoder_off_finished;
+}
+
+
+void encoder_detect_off_finished(void) {
+	for (int i = 0; i < 100; i++) {
+		plot_3data16(0,0,1000);
+		delay_ms(2);
+	}
+	for (int i = 0; i < 1024; i++) {
+		float angle_off = (encoder_off_map[i] + encoder_off_r_map[i]) / 2;
+		plot_3data16(0, 0 , angle_off);
+		delay_ms(2);
+	}
+	for (int i = 0; i < 1024; i++) {
+		float angle_off = (encoder_off_map[i] + encoder_off_r_map[i]) / 2;
+		plot_3data16(0, 0 , angle_off);
+		delay_ms(2);
+
+	}
+	for (int i = 0; i < 1024; i++) {
+		float angle_off = (encoder_off_map[i] + encoder_off_r_map[i]) / 2;
+		plot_3data16(0, 0 , angle_off);
+		delay_ms(2);
+	}	
 }
 
 float encoder_get_vel_count(void) {
@@ -165,8 +257,9 @@ void ENC_TIMER_Overflow(void) {
 
 
 /*PWM 信号捕获一个周期的处理 */
+static int pwm_count = 0;
 void ENC_PWM_Duty_Handler(float t, float d) {
-	float duty = d/t;
+	float duty = ENC_Duty(d, t);
 	if (duty < ENC_PWM_Min_P || duty > 1.0f) {
 		return;
 	}
@@ -174,10 +267,15 @@ void ENC_PWM_Duty_Handler(float t, float d) {
 	if (Nr < 0) {
 		return;
 	}
-	g_encoder.pwm_count = (u32)Nr;
+	u32 n_nr = (u32)Nr;
+	if (Nr - n_nr >= 0.5f) {
+		g_encoder.pwm_count = n_nr + 1;
+	}else {
+		g_encoder.pwm_count = n_nr;
+	}
 	g_encoder.pwm_angle = ENC_Pluse_Nr_2_angle(Nr) * g_encoder.motor_poles + g_encoder.enc_offset;	
 	rand_angle(g_encoder.pwm_angle);
-	if (!g_encoder.b_index_found) {
+	if (!g_encoder.b_index_found && pwm_count++ >= 10) {
 		encoder_sync_pwm_abs();
 	}
 }

Applications/foc/motor/encoder.h

@@ -33,10 +33,13 @@ void encoder_init(void);
 void encoder_init_clear(s8 direction);
 float encoder_get_theta(void);
 float encoder_get_speed(void);
-void encoder_detect_offset(float angle);
+void encoder_detect_offset(float angle, bool r);
 void encoder_set_direction(s8 direction);
 float encoder_get_vel_count(void);
 void encoder_lock_position(bool enable);
+float encoder_detect_finish(bool r);
+float encoder_get_pwm_angle(void);
+void encoder_detect_off_finished(void);
 
 #endif /* _Encoder_H__ */
 

Applications/foc/motor/encoder_off.h

@@ -0,0 +1,113 @@
+#include "os/os_types.h" 
+
+/* Auto gen by my tools, and use matlab to desing a FIR filter-(MC100/Simulink/Fir_angle.m) */
+/* Matlib Command: Fir=Fir_angle; angle_off=filter(Fir.Numerator, 1, angle_in); */
+/* angle_in is the data, by plot MC_tools */
+/* Use angle_out=single(angle_off) convert double to float */
+
+#define FIR_PHASE_SHIFT 542
+const static float _encoder_off_map[] = {
+-11.709626f,-11.025469f,-10.348366f,-9.677769f,-9.012818f,-8.353265f,-7.701627f,-7.057623f,-6.419540f,-5.787866f,-5.163483f,-4.547264f,-3.938902f,-3.337871f,-2.745079f,-2.160679f,
+-1.584892f,-1.016804f,-0.458164f,0.090785f,0.630896f,1.163335f,1.685593f,2.196823f,2.699283f,3.192395f,3.674905f,4.146735f,4.608125f,5.060034f,5.500354f,5.929474f,6.347721f,6.755309f,7.150391f,7.532778f,
+7.904088f,8.263895f,8.611121f,8.945454f,9.267470f,9.577625f,9.874879f,10.158731f,10.430134f,10.688970f,10.934389f,11.165828f,11.383739f,11.588875f,11.780142f,11.956353f,12.118855f,12.267150f,12.401057f,12.519690f,
+12.624034f,12.714013f,12.788465f,12.847351f,12.891816f,12.924930f,12.938406f,12.934103f,12.914685f,12.880808f,12.831861f,12.764849f,12.680910f,12.582322f,12.467851f,12.336022f,12.186927f,12.021371f,11.839053f,11.639188f,
+11.421788f,11.188109f,10.937921f,10.669419f,10.382176f,10.078671f,9.758460f,9.419482f,9.061252f,8.686399f,8.293701f,7.881402f,7.450992f,7.002828f,6.536880f,6.051233f,5.546902f,5.024176f,4.483571f,3.922322f,
+3.342052f,2.744127f,2.127333f,1.490588f,0.834706f,0.160016f,-0.533506f,-1.247272f,-1.980915f,-2.732923f,-3.504051f,-4.295920f,-5.107911f,-5.939118f,-6.788767f,-7.659239f,-8.550577f,-9.460196f,-10.389377f,-11.339470f,
+-12.309464f,-13.299123f,-14.307539f,-15.337227f,-16.387461f,-17.456766f,-18.545980f,-19.655903f,-20.786520f,-21.936377f,-23.106005f,-24.296671f,-25.507820f,-26.738546f,-27.988884f,-29.259979f,-30.552231f,-31.862942f,-33.193447f,-34.544411f,
+-35.915794f,-37.307098f,-38.717583f,-40.148788f,-41.600040f,-43.070999f,-44.561279f,-46.073166f,-47.603806f,-49.154049f,-50.723949f,-52.313599f,-53.923344f,-55.552135f,-57.199631f,-58.867901f,-60.554478f,-62.260494f,-63.985722f,-65.730453f,
+-67.495110f,-69.276863f,-71.077965f,-72.898537f,-74.738159f,-76.595009f,-78.471169f,-80.366554f,-82.279755f,-84.211250f,-86.160439f,-88.128540f,-90.113861f,-92.116684f,-94.137245f,-96.175629f,-98.231926f,-100.304977f,-102.394478f,-104.501663f,
+-106.626846f,-108.766983f,-110.924896f,-113.099243f,-115.290222f,-117.496429f,-119.719383f,-121.958061f,-124.213089f,-126.482582f,-128.767502f,-131.068207f,-133.384430f,-135.714676f,-138.058945f,-140.419128f,-142.793396f,-145.181564f,-147.582993f,-149.999176f,
+-152.428436f,-154.870575f,-157.326584f,-159.795868f,-162.277618f,-164.771729f,-167.277176f,-169.795319f,-172.326309f,-174.867447f,-177.420486f,-179.985596f,-182.561005f,-185.146637f,-187.743423f,-190.351440f,-192.969757f,-195.595917f,-198.232895f,-200.879730f,
+-203.534805f,-206.199112f,-208.871094f,-211.553040f,-214.241730f,-216.938721f,-219.642075f,-222.354797f,-225.074402f,-227.799179f,-230.531021f,-233.270477f,-236.015472f,-238.764801f,-241.520599f,-244.282562f,-247.049118f,-249.819702f,-252.594482f,-255.374161f,
+-258.157928f,-260.944336f,-263.732880f,-266.526733f,-269.323120f,-272.120819f,-274.920105f,-277.722717f,-280.526398f,-283.330719f,-286.136475f,-288.943817f,-291.751312f,-294.557251f,-297.363464f,-300.170135f,-302.976776f,-305.780670f,-308.582703f,-311.384491f,
+-314.183838f,-316.980316f,-319.774109f,-322.566345f,-325.354401f,-328.138641f,-330.919647f,-333.697083f,-336.470367f,-339.237946f,-342.000610f,-344.760040f,-347.513702f,-350.260315f,-353.000885f,-355.735596f,-358.463867f,-361.183929f,-363.897308f,-366.604462f,
+-369.303162f,-371.991913f,-374.673248f,-377.347290f,-380.012543f,-382.665894f,-385.311310f,-387.946838f,-390.572815f,-393.186981f,-395.791077f,-398.384308f,-400.967590f,-403.536804f,-406.095398f,-408.643616f,-411.179138f,-413.700134f,-416.209106f,-418.706268f,
+-421.190002f,-423.658417f,-426.113800f,-428.555817f,-430.983398f,-433.395477f,-435.792511f,-438.176086f,-440.543427f,-442.893921f,-445.228638f,-447.549011f,-449.852661f,-452.138916f,-454.407043f,-456.660370f,-458.896332f,-461.113617f,-463.313049f,-465.496002f,
+-467.660492f,-469.804657f,-471.931122f,-474.039581f,-476.129364f,-478.198242f,-480.248169f,-482.279327f,-484.290894f,-486.280975f,-488.251038f,-490.201294f,-492.131226f,-494.039246f,-495.926239f,-497.793884f,-499.639069f,-501.461517f,-503.263092f,-505.043549f,
+-506.802551f,-508.537628f,-510.250397f,-511.941071f,-513.609192f,-515.253601f,-516.874878f,-518.474792f,-520.050537f,-521.601379f,-523.129456f,-524.635376f,-526.116577f,-527.573364f,-529.005676f,-530.414978f,-531.799927f,-533.159241f,-534.493164f,-535.804443f,
+-537.090942f,-538.350464f,-539.584473f,-540.795166f,-541.980896f,-543.138733f,-544.272156f,-545.380493f,-546.464355f,-547.520508f,-548.550781f,-549.556702f,-550.536377f,-551.488953f,-552.415405f,-553.316406f,-554.191589f,-555.038208f,-555.858887f,-556.654358f,
+-557.423645f,-558.164795f,-558.879456f,-559.568481f,-560.230286f,-560.864807f,-561.473022f,-562.054932f,-562.610107f,-563.137085f,-563.636780f,-564.111206f,-564.558044f,-564.976624f,-565.368408f,-565.734131f,-566.073242f,-566.383423f,-566.666870f,-566.924988f,
+-567.154480f,-567.356567f,-567.531982f,-567.681030f,-567.802673f,-567.896118f,-567.963257f,-568.004028f,-568.018494f,-568.004211f,-567.962402f,-567.895752f,-567.801086f,-567.678650f,-567.529358f,-567.355103f,-567.153320f,-566.923401f,-566.667358f,-566.385559f,
+-566.077637f,-565.741699f,-565.379089f,-564.991272f,-564.576355f,-564.134338f,-563.666199f,-563.173096f,-562.653870f,-562.107300f,-561.534851f,-560.937195f,-560.314697f,-559.664185f,-558.988831f,-558.288330f,-557.562439f,-556.810547f,-556.032898f,-555.232117f,
+-554.405273f,-553.552979f,-552.675903f,-551.775269f,-550.849609f,-549.898254f,-548.921753f,-547.923218f,-546.900208f,-545.851746f,-544.779358f,-543.684631f,-542.565613f,-541.421936f,-540.255615f,-539.066772f,-537.854858f,-536.618347f,-535.359009f,-534.078308f,
+-532.775269f,-531.447998f,-530.098511f,-528.728394f,-527.335815f,-525.920532f,-524.483643f,-523.026672f,-521.547363f,-520.046692f,-518.525024f,-516.983582f,-515.421997f,-513.837952f,-512.234375f,-510.611786f,-508.969757f,-507.306549f,-505.624268f,-503.923950f,
+-502.203217f,-500.462891f,-498.704224f,-496.928436f,-495.133698f,-493.319214f,-491.487610f,-489.639587f,-487.773895f,-485.890350f,-483.989441f,-482.072876f,-480.139130f,-478.188599f,-476.221130f,-474.239746f,-472.241943f,-470.227905f,-468.198730f,-466.155548f,
+-464.097626f,-462.023346f,-459.935913f,-457.835144f,-455.719849f,-453.589844f,-451.448059f,-449.292816f,-447.125183f,-444.943848f,-442.750214f,-440.545258f,-438.328400f,-436.098816f,-433.857910f,-431.606567f,-429.343811f,-427.070160f,-424.786041f,-422.492462f,
+-420.188995f,-417.875397f,-415.552216f,-413.221008f,-410.881256f,-408.531860f,-406.174316f,-403.809692f,-401.436707f,-399.055359f,-396.667236f,-394.272278f,-391.871521f,-389.463837f,-387.048676f,-384.628998f,-382.203033f,-379.771240f,-377.334442f,-374.894012f,
+-372.448975f,-369.997345f,-367.543030f,-365.086029f,-362.625214f,-360.160004f,-357.691772f,-355.222443f,-352.749420f,-350.273499f,-347.795837f,-345.317993f,-342.838318f,-340.355988f,-337.873688f,-335.391571f,-332.909760f,-330.426300f,-327.943695f,-325.462494f,
+-322.982056f,-320.501129f,-318.020905f,-315.544525f,-313.069305f,-310.594360f,-308.121643f,-305.653564f,-303.187439f,-300.723114f,-298.262543f,-295.806488f,-293.353790f,-290.904022f,-288.459137f,-286.019989f,-283.584747f,-281.153717f,-278.728119f,-276.309235f,
+-273.896057f,-271.487183f,-269.085114f,-266.690247f,-264.301270f,-261.919098f,-259.543823f,-257.177185f,-254.817230f,-252.463165f,-250.118103f,-247.782730f,-245.454666f,-243.135101f,-240.824066f,-238.522385f,-236.230026f,-233.945633f,-231.670990f,-229.407822f,
+-227.153198f,-224.908463f,-222.674454f,-220.452560f,-218.240707f,-216.038025f,-213.847168f,-211.668274f,-209.500839f,-207.343262f,-205.198135f,-203.065948f,-200.945969f,-198.836823f,-196.740417f,-194.658218f,-192.587585f,-190.528305f,-188.482742f,-186.451584f,
+-184.433502f,-182.427780f,-180.435303f,-178.457108f,-176.493637f,-174.542542f,-172.605606f,-170.683838f,-168.776184f,-166.881790f,-165.001236f,-163.137299f,-161.287994f,-159.452133f,-157.630859f,-155.825745f,-154.036346f,-152.260880f,-150.500580f,-148.756393f,
+-147.027695f,-145.314209f,-143.615997f,-141.934830f,-140.269577f,-138.618546f,-136.984344f,-135.366531f,-133.765305f,-132.178925f,-130.608200f,-129.054916f,-127.518303f,-125.996613f,-124.492020f,-123.004158f,-121.532761f,-120.077271f,-118.638313f,-117.216454f,
+-115.811943f,-114.422096f,-113.049606f,-111.694817f,-110.356819f,-109.034401f,-107.728554f,-106.440430f,-105.168579f,-103.912804f,-102.674202f,-101.453247f,-100.248528f,-99.060402f,-97.888435f,-96.734711f,-95.597740f,-94.476334f,-93.371437f,-92.284706f,
+-91.214447f,-90.160225f,-89.123138f,-88.103241f,-87.099228f,-86.111465f,-85.140297f,-84.186501f,-83.249084f,-82.327766f,-81.422546f,-80.533691f,-79.661690f,-78.806335f,-77.966835f,-77.143478f,-76.336441f,-75.545403f,-74.770126f,-74.011322f,
+-73.268898f,-72.541702f,-71.830467f,-71.134972f,-70.455704f,-69.791977f,-69.143463f,-68.510445f,-67.892914f,-67.290718f,-66.704002f,-66.132164f,-65.575508f,-65.033676f,-64.506416f,-63.995049f,-63.498386f,-63.015713f,-62.547852f,-62.094040f,
+-61.655296f,-61.231045f,-60.820362f,-60.424366f,-60.041824f,-59.673569f,-59.319077f,-58.978745f,-58.651791f,-58.338249f,-58.037426f,-57.750774f,-57.477211f,-57.216255f,-56.968796f,-56.733860f,-56.511642f,-56.302219f,-56.105347f,-55.920872f,
+-55.748566f,-55.587971f,-55.439575f,-55.303982f,-55.180054f,-55.066784f,-54.965435f,-54.875675f,-54.797485f,-54.730385f,-54.674557f,-54.628883f,-54.593964f,-54.570011f,-54.556816f,-54.554600f,-54.561714f,-54.578690f,-54.605755f,-54.642677f,
+-54.689743f,-54.746094f,-54.812191f,-54.886791f,-54.969975f,-55.062881f,-55.164520f,-55.275143f,-55.393478f,-55.519936f,-55.655125f,-55.798122f,-55.949356f,-56.108078f,-56.274170f,-56.447247f,-56.628143f,-56.816505f,-57.011753f,-57.213623f,
+-57.421326f,-57.636356f,-57.858585f,-58.086452f,-58.320290f,-58.559464f,-58.804932f,-59.055832f,-59.311897f,-59.573738f,-59.840584f,-60.111217f,-60.387074f,-60.667629f,-60.952724f,-61.241634f,-61.534550f,-61.831490f,-62.131931f,-62.435371f,
+-62.742779f,-63.053482f,-63.367031f,-63.682964f,-64.002472f,-64.324692f,-64.648911f,-64.974899f,-65.303162f,-65.633858f,-65.966164f,-66.299866f,-66.634254f,-66.970535f,-67.308296f,-67.645905f,-67.984917f,-68.324753f,-68.664421f,-69.003670f,
+-69.343285f,-69.683434f,-70.022949f,-70.362022f,-70.699684f,-71.037010f,-71.373901f,-71.708755f,-72.043030f,-72.375946f,-72.706612f,-73.035133f,-73.361938f,-73.687401f,-74.010368f,-74.330612f,-74.647964f,-74.963127f,-75.275627f,-75.584633f,
+-75.890541f,-76.193123f,-76.491920f,-76.786980f,-77.078911f,-77.367119f,-77.650749f,-77.930038f,-78.204872f,-78.475441f,-78.742180f,-79.003029f,-79.258797f,-79.510101f,-79.756073f,-79.996208f,-80.231224f,-80.460915f,-80.685066f,-80.902519f,
+-81.113480f,-81.319969f,-81.519775f,-81.712776f,-81.899330f,-82.079681f,-82.253418f,-82.419319f,-82.578865f,-82.732193f,-82.877663f,-83.015427f,-83.145454f,-83.268776f,-83.384834f,-83.492203f,-83.591492f,-83.683594f,-83.768295f,-83.844681f,
+-83.912437f,-83.972595f,-84.024567f,-84.067795f,-84.102570f,-84.129456f,-84.146973f,-84.155983f,-84.155533f,-84.147156f,-84.130379f,-84.104202f,-84.068558f,-84.024422f,-83.971382f,-83.909302f,-83.838104f,-83.757675f,-83.668251f,-83.569092f,
+-83.460289f,-83.342751f,-83.216209f,-83.079285f,-82.932533f,-82.777420f,-82.613358f,-82.439163f,-82.255402f,-82.061905f,-81.859207f,-81.647820f,-81.425880f,-81.194778f,-80.954567f,-80.704720f,-80.444397f,-80.175919f,-79.897919f,-79.610397f,
+-79.312943f,-79.005951f,-78.690231f,-78.365265f,-78.030663f,-77.686623f,-77.333366f,-76.971649f,-76.599930f,-76.219109f,-75.830101f,-75.432304f,-75.025024f,-74.607948f,-74.182907f,-73.749290f,-73.306320f,-72.854652f,-72.395218f,-71.927162f,
+-71.450928f,-70.965927f,-70.472313f,-69.971291f,-69.461472f,-68.943710f,-68.418976f,-67.886147f,-67.345665f,-66.796753f,-66.241539f,-65.679337f,-65.108963f,-64.531746f,-63.947582f,-63.355984f,-62.757576f,-62.151688f,-61.540573f,-60.923683f,
+-60.299202f,-59.667603f,-59.031235f,-58.389729f,-57.741520f,-57.086998f,-56.427406f,-55.762924f,-55.093052f,-54.417397f,-53.736542f,-53.052391f,-52.362637f,-51.667618f,-50.968018f,-50.265739f,-49.558769f,-48.846355f,-48.131161f,-47.413334f,
+-46.691826f,-45.966240f,-45.237850f,-44.507519f,-43.773808f,-43.037075f,-42.297630f,-41.557686f,-40.814732f,-40.068508f,-39.320545f,-38.572918f,-37.823395f,-37.071384f,-36.318882f,-35.566235f,-34.812309f,-34.056995f,-33.301933f,-32.548576f,
+-31.794197f,-31.038944f,-30.284048f,-29.530386f,-28.778368f,-28.025517f,-27.274679f,-26.525764f,-25.778835f,-25.031500f,-24.286755f,-23.545662f,-22.806223f,-22.067678f,-21.332323f,-20.601440f,-19.872141f,-19.145342f,-18.422464f,-17.705357f,
+-16.990997f,-16.278864f,-15.571356f,-14.869671f,-14.171355f,-13.476915f,-12.787113f,-12.104268f,-11.425644f,-10.751867f,-10.084347f,-9.423292f,-8.768794f,-8.118172f,-7.475220f,-6.839772f,-6.211228f,-5.588218f,-4.971616f,-4.363459f,
+-3.763738f,-3.170528f,-2.584927f,-2.007931f,-1.439904f,-0.878003f,-0.325492f,0.217171f,0.750049f,1.276983f,1.794182f,2.299769f,2.795597f,3.283551f,3.761352f,4.227379f,4.682990f,5.130264f,5.566881f,5.991579f,
+6.404877f,6.808776f,7.201293f,7.580972f,7.948988f,8.305903f,8.651197f,8.982660f,9.302001f,9.609636f,9.905892f,10.187147f,10.455523f,10.712256f,10.956389f,11.186290f,11.401634f,11.605371f,11.795970f,11.970403f,
+12.130858f,12.278466f,12.412258f,12.529469f,12.632153f,12.721844f,12.796059f,12.854102f,12.896466f,12.924930f,12.938406f,12.934103f,12.914685f,12.880808f,12.831861f,12.764849f,12.680910f,12.582322f,12.467851f,12.336022f,
+12.186927f,12.021371f,11.839053f,11.639188f,11.421788f,11.188109f,10.937921f,10.669419f,10.382176f,10.078671f,9.758460f,9.419482f,9.061252f,8.686399f,8.293701f,7.881402f,7.450992f,7.002828f,6.536880f,6.051233f,
+5.546902f,5.024176f,4.483571f,3.922322f,3.342052f,2.744127f,2.127333f,1.490588f,0.834706f,0.160016f,-0.533506f,-1.247272f,-1.980915f,-2.732923f,-3.504051f,-4.295920f,-5.107911f,-5.939118f,-6.788767f,-7.659239f,
+-8.550577f,-9.460196f,-10.389377f,-11.339470f,-12.309464f,-13.299123f,-14.307539f,-15.337227f,-16.387461f,-17.456766f,-18.545980f,-19.655903f,-20.786520f,-21.936377f,-23.106005f,-24.296671f,-25.507820f,-26.738546f,-27.988884f,-29.259979f,
+-30.552231f,-31.862942f,-33.193447f,-34.544411f,-35.915794f,-37.307098f,-38.717583f,-40.148788f,-41.600040f,-43.070999f,-44.561279f,-46.073166f,-47.603806f,-49.154049f,-50.723949f,-52.313599f,-53.923344f,-55.552135f,-57.199631f,-58.867901f,
+-60.554478f,-62.260494f,-63.985722f,-65.730453f,-67.495110f,-69.276863f,-71.077965f,-72.898537f,-74.738159f,-76.595009f,-78.471169f,-80.366554f,-82.279755f,-84.211250f,-86.160439f,-88.128540f,-90.113861f,-92.116684f,-94.137245f,-96.175629f,
+-98.231926f,-100.304977f,-102.394478f,-104.501663f,-106.626846f,-108.766983f,-110.924896f,-113.099243f,-115.290222f,-117.496429f,-119.719383f,-121.958061f,-124.213089f,-126.482582f,-128.767502f,-131.068207f,-133.384430f,-135.714676f,-138.058945f,-140.419128f,
+-142.793396f,-145.181564f,-147.582993f,-149.999176f,-152.428436f,-154.870575f,-157.326584f,-159.795868f,-162.277618f,-164.771729f,-167.277176f,-169.795319f,-172.326309f,-174.867447f,-177.420486f,-179.985596f,-182.561005f,-185.146637f,-187.743423f,-190.351440f,
+-192.969757f,-195.595917f,-198.232895f,-200.879730f,-203.534805f,-206.199112f,-208.871094f,-211.553040f,-214.241730f,-216.938721f,-219.642075f,-222.354797f,-225.074402f,-227.799179f,-230.531021f,-233.270477f,-236.015472f,-238.764801f,-241.520599f,-244.282562f,
+-247.049118f,-249.819702f,-252.594482f,-255.374161f,-258.157928f,-260.944336f,-263.732880f,-266.526733f,-269.323120f,-272.120819f,-274.920105f,-277.722717f,-280.526398f,-283.330719f,-286.136475f,-288.943817f,-291.751312f,-294.557251f,-297.363464f,-300.170135f,
+-302.976776f,-305.780670f,-308.582703f,-311.384491f,-314.183838f,-316.980316f,-319.774109f,-322.566345f,-325.354401f,-328.138641f,-330.919647f,-333.697083f,-336.470367f,-339.237946f,-342.000610f,-344.760040f,-347.513702f,-350.260315f,-353.000885f,-355.735596f,
+-358.463867f,-361.183929f,-363.897308f,-366.604462f,-369.303162f,-371.991913f,-374.673248f,-377.347290f,-380.012543f,-382.665894f,-385.311310f,-387.946838f,-390.572815f,-393.186981f,-395.791077f,-398.384308f,-400.967590f,-403.536804f,-406.095398f,-408.643616f,
+-411.179138f,-413.700134f,-416.209106f,-418.706268f,-421.190002f,-423.658417f,-426.113800f,-428.555817f,-430.983398f,-433.395477f,-435.792511f,-438.176086f,-440.543427f,-442.893921f,-445.228638f,-447.549011f,-449.852661f,-452.138916f,-454.407043f,-456.660370f,
+-458.896332f,-461.113617f,-463.313049f,-465.496002f,-467.660492f,-469.804657f,-471.931122f,-474.039581f,-476.129364f,-478.198242f,-480.248169f,-482.279327f,-484.290894f,-486.280975f,-488.251038f,-490.201294f,-492.131226f,-494.039246f,-495.926239f,-497.793884f,
+-499.639069f,-501.461517f,-503.263092f,-505.043549f,-506.802551f,-508.537628f,-510.250397f,-511.941071f,-513.609192f,-515.253601f,-516.874878f,-518.474792f,-520.050537f,-521.601379f,-523.129456f,-524.635376f,-526.116577f,-527.573364f,-529.005676f,-530.414978f,
+-531.799927f,-533.159241f,-534.493164f,-535.804443f,-537.090942f,-538.350464f,-539.584473f,-540.795166f,-541.980896f,-543.138733f,-544.272156f,-545.380493f,-546.464355f,-547.520508f,-548.550781f,-549.556702f,-550.536377f,-551.488953f,-552.415405f,-553.316406f,
+-554.191589f,-555.038208f,-555.858887f,-556.654358f,-557.423645f,-558.164795f,-558.879456f,-559.568481f,-560.230286f,-560.864807f,-561.473022f,-562.054932f,-562.610107f,-563.137085f,-563.636780f,-564.111206f,-564.558044f,-564.976624f,-565.368408f,-565.734131f,
+-566.073242f,-566.383423f,-566.666870f,-566.924988f,-567.154480f,-567.356567f,-567.531982f,-567.681030f,-567.802673f,-567.896118f,-567.963257f,-568.004028f,-568.018494f,-568.004211f,-567.962402f,-567.895752f,-567.801086f,-567.678650f,-567.529358f,-567.355103f,
+-567.153320f,-566.923401f,-566.667358f,-566.385559f,-566.077637f,-565.741699f,-565.379089f,-564.991272f,-564.576355f,-564.134338f,-563.666199f,-563.173096f,-562.653870f,-562.107300f,-561.534851f,-560.937195f,-560.314697f,-559.664185f,-558.988831f,-558.288330f,
+-557.562439f,-556.810547f,-556.032898f,-555.232117f,-554.405273f,-553.552979f,-552.675903f,-551.775269f,-550.849609f,-549.898254f,-548.921753f,-547.923218f,-546.900208f,-545.851746f,-544.779358f,-543.684631f,-542.565613f,-541.421936f,-540.255615f,-539.066772f,
+-537.854858f,-536.618347f,-535.359009f,-534.078308f,-532.775269f,-531.447998f,-530.098511f,-528.728394f,-527.335815f,-525.920532f,-524.483643f,-523.026672f,-521.547363f,-520.046692f,-518.525024f,-516.983582f,-515.421997f,-513.837952f,-512.234375f,-510.611786f,
+-508.969757f,-507.306549f,-505.624268f,-503.923950f,-502.203217f,-500.462891f,-498.704224f,-496.928436f,-495.133698f,-493.319214f,-491.487610f,-489.639587f,-487.773895f,-485.890350f,-483.989441f,-482.072876f,-480.139130f,-478.188599f,-476.221130f,-474.239746f,
+-472.241943f,-470.227905f,-468.198730f,-466.155548f,-464.097626f,-462.023346f,-459.935913f,-457.835144f,-455.719849f,-453.589844f,-451.448059f,-449.292816f,-447.125183f,-444.943848f,-442.750214f,-440.545258f,-438.328400f,-436.098816f,-433.857910f,-431.606567f,
+-429.343811f,-427.070160f,-424.786041f,-422.492462f,-420.188995f,-417.875397f,-415.552216f,-413.221008f,-410.881256f,-408.531860f,-406.174316f,-403.809692f,-401.436707f,-399.055359f,-396.667236f,-394.272278f,-391.871521f,-389.463837f,-387.048676f,-384.628998f,
+-382.203033f,-379.771240f,-377.334442f,-374.894012f,-372.448975f,-369.997345f,-367.543030f,-365.086029f,-362.625214f,-360.160004f,-357.691772f,-355.222443f,-352.749420f,-350.273499f,-347.795837f,-345.317993f,-342.838318f,-340.355988f,-337.873688f,-335.391571f,
+-332.909760f,-330.426300f,-327.943695f,-325.462494f,-322.982056f,-320.501129f,-318.020905f,-315.544525f,-313.069305f,-310.594360f,-308.121643f,-305.653564f,-303.187439f,-300.723114f,-298.262543f,-295.806488f,-293.353790f,-290.904022f,-288.459137f,-286.019989f,
+-283.584747f,-281.153717f,-278.728119f,-276.309235f,-273.896057f,-271.487183f,-269.085114f,-266.690247f,-264.301270f,-261.919098f,-259.543823f,-257.177185f,-254.817230f,-252.463165f,-250.118103f,-247.782730f,-245.454666f,-243.135101f,-240.824066f,-238.522385f,
+-236.230026f,-233.945633f,-231.670990f,-229.407822f,-227.153198f,-224.908463f,-222.674454f,-220.452560f,-218.240707f,-216.038025f,-213.847168f,-211.668274f,-209.500839f,-207.343262f,-205.198135f,-203.065948f,-200.945969f,-198.836823f,-196.740417f,-194.658218f,
+-192.587585f,-190.528305f,-188.482742f,-186.451584f,-184.433502f,-182.427780f,-180.435303f,-178.457108f,-176.493637f,-174.542542f,-172.605606f,-170.683838f,-168.776184f,-166.881790f,-165.001236f,-163.137299f,-161.287994f,-159.452133f,-157.630859f,-155.825745f,
+-154.036346f,-152.260880f,-150.500580f,-148.756393f,-147.027695f,-145.314209f,-143.615997f,-141.934830f,-140.269577f,-138.618546f,-136.984344f,-135.366531f,-133.765305f,-132.178925f,-130.608200f,-129.054916f,-127.518303f,-125.996613f,-124.492020f,-123.004158f,
+-121.532761f,-120.077271f,-118.638313f,-117.216454f,-115.811943f,-114.422096f,-113.049606f,-111.694817f,-110.356819f,-109.034401f,-107.728554f,-106.440430f,-105.168579f,-103.912804f,-102.674202f,-101.453247f,-100.248528f,-99.060402f,-97.888435f,-96.734711f,
+-95.597740f,-94.476334f,-93.371437f,-92.284706f,-91.214447f,-90.160225f,-89.123138f,-88.103241f,-87.099228f,-86.111465f,-85.140297f,-84.186501f,-83.249084f,-82.327766f,-81.422546f,-80.533691f,-79.661690f,-78.806335f,-77.966835f,-77.143478f,
+-76.336441f,-75.545403f,-74.770126f,-74.011322f,-73.268898f,-72.541702f,-71.830467f,-71.134972f,-70.455704f,-69.791977f,-69.143463f,-68.510445f,-67.892914f,-67.290718f,-66.704002f,-66.132164f,-65.575508f,-65.033676f,-64.506416f,-63.995049f,
+-63.498386f,-63.015713f,-62.547852f,-62.094040f,-61.655296f,-61.231045f,-60.820362f,-60.424366f,-60.041824f,-59.673569f,-59.319077f,-58.978745f,-58.651791f,-58.338249f,-58.037426f,-57.750774f,-57.477211f,-57.216255f,-56.968796f,-56.733860f,
+-56.511642f,-56.302219f,-56.105347f,-55.920872f,-55.748566f,-55.587971f,-55.439575f,-55.303982f,-55.180054f,-55.066784f,-54.965435f,-54.875675f,-54.797485f,-54.730385f,-54.674557f,-54.628883f,-54.593964f,-54.570011f,-54.556816f,-54.554600f,
+-54.561714f,-54.578690f,-54.605755f,-54.642677f,-54.689743f,-54.746094f,-54.812191f,-54.886791f,-54.969975f,-55.062881f,-55.164520f,-55.275143f,-55.393478f,-55.519936f,-55.655125f,-55.798122f,-55.949356f,-56.108078f,-56.274170f,-56.447247f,
+-56.628143f,-56.816505f,-57.011753f,-57.213623f,-57.421326f,-57.636356f,-57.858585f,-58.086452f,-58.320290f,-58.559464f,-58.804932f,-59.055832f,-59.311897f,-59.573738f,-59.840584f,-60.111217f,-60.387074f,-60.667629f,-60.952724f,-61.241634f,
+-61.534550f,-61.831490f,-62.131931f,-62.435371f,-62.742779f,-63.053482f,-63.367031f,-63.682964f,-64.002472f,-64.324692f,-64.648911f,-64.974899f,-65.303162f,-65.633858f,-65.966164f,-66.299866f,-66.634254f,-66.970535f,-67.308296f,-67.645905f,
+-67.984917f,-68.324753f,-68.664421f,-69.003670f,-69.343285f,-69.683434f,-70.022949f,-70.362022f,-70.699684f,-71.037010f,-71.373901f,-71.708755f,-72.043030f,-72.375946f,-72.706612f,-73.035133f,-73.361938f,-73.687401f,-74.010368f,-74.330612f,
+-74.647964f,-74.963127f,-75.275627f,-75.584633f,-75.890541f,-76.193123f,-76.491920f,-76.786980f,-77.078911f,-77.367119f,-77.650749f,-77.930038f,-78.204872f,-78.475441f,-78.742180f,-79.003029f,-79.258797f,-79.510101f,-79.756073f,-79.996208f,
+-80.231224f,-80.460915f,-80.685066f,-80.902519f,-81.113480f,-81.319969f,-81.519775f,-81.712776f,-81.899330f,-82.079681f,-82.253418f,-82.419319f,-82.578865f,-82.732193f,-82.877663f,-83.015427f,-83.145454f,-83.268776f,-83.384834f,-83.492203f,
+-83.591492f,-83.683594f,-83.768295f,-83.844681f,-83.912437f,-83.972595f,-84.024567f,-84.067795f,-84.102570f,-84.129456f,-84.146973f,-84.155983f,-84.155533f,-84.147156f,-84.130379f,-84.104202f,-84.068558f,-84.024422f,-83.971382f,-83.909302f,
+-83.838104f,-83.757675f,-83.668251f,-83.569092f,-83.460289f,-83.342751f,-83.216209f,-83.079285f,-82.932533f,-82.777420f,-82.613358f,-82.439163f,-82.255402f,-82.061905f,-81.859207f,-81.647820f,-81.425880f,-81.194778f,-80.954567f,-80.704720f,
+-80.444397f,-80.175919f,-79.897919f,-79.610397f,-79.312943f,-79.005951f,-78.690231f,-78.365265f,-78.030663f,-77.686623f,-77.333366f,-76.971649f,-76.599930f,-76.219109f,-75.830101f,-75.432304f,-75.025024f,-74.607948f,-74.182907f,-73.749290f,
+-73.306320f,-72.854652f,-72.395218f,-71.927162f,-71.450928f,-70.965927f,-70.472313f,-69.971291f,-69.461472f,-68.943710f,-68.418976f,-67.886147f,-67.345665f,-66.796753f,-66.241539f,-65.679337f,-65.108963f,-64.531746f,-63.947582f,-63.355984f,
+-62.757576f,-62.151688f,-61.540573f,-60.923683f,-60.299202f,-59.667603f,-59.031235f,-58.389729f,-57.741520f,-57.086998f,-56.427406f,-55.762924f,-55.093052f,-54.417397f,-53.736542f,-53.052391f,-52.362637f,-51.667618f,-50.968018f,-50.265739f,
+-49.558769f,-48.846355f,-48.131161f,-47.413334f,-46.691826f,-45.966240f,-45.237850f,-44.507519f,-43.773808f,-43.037075f,-42.297630f,-41.557686f,-40.814732f,-40.068508f,-39.320545f,-38.572918f,-37.823395f,-37.071384f,-36.318882f,-35.566235f,
+-34.812309f,-34.056995f,-33.301933f,-32.548576f,-31.794197f,-31.038944f,-30.284048f,-29.530386f,-28.778368f,-28.025517f,-27.274679f,-26.525764f,-25.778835f,-25.031500f,-24.286755f,-23.545662f,-22.806223f,-22.067678f,-21.332323f,-20.601440f,
+-19.872141f,-19.145342f,-18.422464f,-17.705357f,-16.990997f,-16.278864f,-15.571356f,-14.869671f,-14.171355f,-13.476915f,-12.787113f,-12.104268f,
+};

Applications/foc/motor/motor.c

@@ -127,23 +127,36 @@ void mc_encoder_calibrate(s16 vd) {
 	phase_current_calibrate_wait();
 	PMSM_FOC_Set_Angle(0);
 	PMSM_FOC_SetOpenVdq(vd, 0);
-	delay_ms(3000);
-	for (int i = 0; i < 50000000; i++) {
-		for (s16 angle = 0; angle < 360; angle++) {
+	delay_ms(2000);
+	for (int i = 0; i < 1000000; i++) {
+		for (float angle = 0; angle < 360; angle++) {
 			PMSM_FOC_Set_Angle(angle);
-			
-			motor_encoder_offset(angle);
-			
-			delay_us(1000);
+			delay_ms(2);
+			encoder_detect_offset(angle, false);
+		}
+		if (encoder_detect_finish(false)) {
+			break;
 		}
 	}
 	delay_ms(500);
+	for (int i = 0; i < 1000000; i++) {
+		for (float angle = 359; angle >= 0; angle--) {
+			PMSM_FOC_Set_Angle(angle);
+			delay_ms(2);
+			encoder_detect_offset(angle, true);
+		}
+		if (encoder_detect_finish(true)) {
+			break;
+		}
+	}
+	encoder_detect_off_finished();
+	delay_ms(500);
 	PMSM_FOC_SetOpenVdq(0, 0);
 	delay_ms(500);
 	adc_stop_convert();
 	pwm_stop();
 	PMSM_FOC_Stop();
-	motor_encoder_offset_finish();
+	
 }
 
 bool mc_current_sensor_calibrate(float current) {
@@ -227,6 +240,11 @@ void ADC_IRQHandler(void) {
 	if (phase_current_offset()) {//check if is adc offset checked
 		return;
 	}
+	if (phase_current_sensor_do_calibrate()){
+		pwm_update_duty(100, FOC_PWM_Half_Period-100, 100);
+		pwm_update_sample(FOC_PWM_Half_Period-1, FOC_PWM_Half_Period+1, PHASE_BC);
+		return;
+	}
 	TIME_MEATURE_START();
 	PMSM_FOC_Schedule();
 	TIME_MEATURE_END();

Applications/foc/motor/motor.h

@@ -79,7 +79,7 @@ static __INLINE void motor_encoder_offset(float angle) {
 #ifdef USE_ENCODER_HALL
 		hall_detect_offset(angle);
 #elif defined (USE_ENCODER_ABI)
-		encoder_detect_offset(angle);
+		encoder_detect_offset(angle, false);
 #else
 	#error "Postion sensor ERROR"
 #endif
@@ -89,6 +89,7 @@ static __INLINE void motor_encoder_offset_finish(void) {
 #ifdef USE_ENCODER_HALL
 		hall_detect_offset_finish();
 #elif defined (USE_ENCODER_ABI)
+		encoder_detect_finish(false);
 #else
 	#error "Postion sensor ERROR"
 #endif

Applications/foc/motor/motor_param.h

@@ -6,29 +6,20 @@
 #define MOTOR_Ld   0.00136f
 #define MOTOR_Lq   0.00136f
 #define MOTOR_POLES     2
-#else
+#elif 0
 //编码器电机 3505
 #define MOTOR_R 0.08f
 #define MOTOR_Ld 0.000032f
 #define MOTOR_Lq 0.000032f
 #define MOTOR_POLES  10
+#else
+#define MOTOR_R 0.012f
+#define MOTOR_Ld (0.000143f*0.5f)
+#define MOTOR_Lq (0.000205f*0.5f)
+#define MOTOR_POLES  5
 #endif
 
-/* 编码器参数      */
-#define ENC_MAX_RES  4096
-/*min. 490 Hz, max 603 Hz*/
-#define ENC_PWM_Max_P (1.0f/490.0f)
-#define ENC_PWM_Min_P (1.0f/603.0f)
-
-#define ENC_PWM_MAX_RES 4119.0F
-#define ENC_PWM_INIT_WIDTH 12.0F //PWM 起始宽度
-#define ENC_PWM_ERROR_WIDTH 4.0f //PWM 指示错误的宽度
-#define ENC_PWM_END_WIDTH   0.0F
-#define ENC_PWM_Error_P (ENC_PWM_INIT_WIDTH/ENC_PWM_MAX_RES)
-#define ENC_PWM_MIN_duty ((ENC_PWM_INIT_WIDTH + ENC_PWM_ERROR_WIDTH + ENC_PWM_END_WIDTH)/ENC_PWM_MAX_RES)
 
-#define ENC_Duty_2_Pluse_Nr(duty) (duty * ENC_PWM_MAX_RES - (ENC_PWM_INIT_WIDTH + ENC_PWM_ERROR_WIDTH + ENC_PWM_END_WIDTH)) //通过占空比计算有几个脉冲
-#define ENC_Pluse_Nr_2_angle(Nr) (360.0f/(float)ENC_MAX_RES * (Nr))
 
 
 #endif /* _MOTOR_PARAM_H__ */

Applications/math/sin_table.c

@@ -1,5 +1,6 @@
 #include "os/os_types.h"
 #include "math/fix_math.h"
+#include "arm_math.h"
 /*
 1.cos和sin转换公式一
 
@@ -13,6 +14,7 @@ cos[(pi/2)+x]=-sinx；
 
 sin[(pi/2)+x]=cosx；
 */
+#if 0
 static s16 sinTable[] = 
 { 0, 286, 572, 857, 1143, 1428, 1713, 1997, 2280, 2563, 2845, 3126, 3406, 3686,
   3964, 4240, 4516, 4790, 5063, 5334, 5604, 5872, 6138, 6402, 6664, 6924, 7182,
@@ -49,7 +51,7 @@ static s16 sinTable[] =
   -5334, -5063, -4790, -4516, -4240, -3964, -3686, -3406, -3126, -2845, -2563,
   -2280, -1997, -1713, -1428, -1143, -857, -572, -286, 0 };
 
-void SinCos_Lut(float angle, float *s, float *c) {
+void SinCos_Lut(float angle, float *s, float *c) {	
 	s16 angle_degree = (s16)angle; //去掉小数部分
 	if ((angle - angle_degree) > 0.5f) {
 		angle_degree += 1;
@@ -60,4 +62,9 @@ void SinCos_Lut(float angle, float *s, float *c) {
 	angle_degree = (angle_degree + 90) % 360;
 	*c = S16Q14toF(sinTable[angle_degree]);
 }
+#else
+void SinCos_Lut(float angle, float *s, float *c) {
+	arm_sin_cos_f32(angle, s, c);
+}
+#endif
 

Applications/os/heap_4.c

@@ -38,7 +38,7 @@
 
 #define portBYTE_ALIGNMENT			8
 #define portBYTE_ALIGNMENT_MASK 	0x0007
-#define configTOTAL_HEAP_SIZE    (10*1024)
+#define configTOTAL_HEAP_SIZE    (6*1024)
 #define configASSERT(x)
 /* Block sizes must not get too small. */
 #define heapMINIMUM_BLOCK_SIZE	( ( size_t ) ( xHeapStructSize << 1 ) )

Project/MC100.uvoptx

@@ -117,6 +117,10 @@
         <pMon>Segger\JL2CM3.dll</pMon>
       </DebugOpt>
       <TargetDriverDllRegistry>
+        <SetRegEntry>
+          <Number>0</Number>
+          <Key>DLGUARM</Key>
+        </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>ARMRTXEVENTFLAGS</Key>
@@ -132,14 +136,10 @@
           <Key>ARMDBGFLAGS</Key>
           <Name></Name>
         </SetRegEntry>
-        <SetRegEntry>
-          <Number>0</Number>
-          <Key>DLGUARM</Key>
-        </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>JL2CM3</Key>
-          <Name>-U81480416 -O78 -S0 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST1 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO7 -FD20000000 -FC1000 -FN1 -FF0GD32F30x_HD.FLM -FS08000000 -FL080000 -FP0($$Device:GD32F303RC$Flash\GD32F30x_HD.FLM)</Name>
+          <Name>-U81480416 -O78 -S2 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST1 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO7 -FD20000000 -FC1000 -FN1 -FF0GD32F30x_HD.FLM -FS08000000 -FL080000 -FP0($$Device:GD32F303RC$Flash\GD32F30x_HD.FLM)</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -157,12 +157,72 @@
         <Ww>
           <count>1</count>
           <WinNumber>1</WinNumber>
-          <ItemText>jtag_cmd</ItemText>
+          <ItemText>_gFOC_Ctrl,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>2</count>
           <WinNumber>1</WinNumber>
-          <ItemText>_gFOC_Ctrl,0x0A</ItemText>
+          <ItemText>g_meas_foc,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>3</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>jtag_data,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>4</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>jtag_cmd,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>5</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>g_encoder,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>6</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>encoder_off_count</ItemText>
+        </Ww>
+        <Ww>
+          <count>7</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>encoder_off_map,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>8</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>encoder_off_cnt,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>9</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>encoder_off_finished</ItemText>
+        </Ww>
+        <Ww>
+          <count>10</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>encoder_off_comp,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>11</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>encoder_off_mul</ItemText>
+        </Ww>
+        <Ww>
+          <count>12</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>encoder_off_add,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>13</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>encoder_off_cn_add,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>14</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>encoder_off_angle_add</ItemText>
         </Ww>
       </WatchWindow1>
       <Tracepoint>
@@ -207,6 +267,24 @@
       <pszMrulep></pszMrulep>
       <pSingCmdsp></pSingCmdsp>
       <pMultCmdsp></pMultCmdsp>
+      <SystemViewers>
+        <Entry>
+          <Name>System Viewer\TIMER0</Name>
+          <WinId>35903</WinId>
+        </Entry>
+        <Entry>
+          <Name>System Viewer\TIMER1</Name>
+          <WinId>35905</WinId>
+        </Entry>
+        <Entry>
+          <Name>System Viewer\TIMER2</Name>
+          <WinId>35904</WinId>
+        </Entry>
+        <Entry>
+          <Name>System Viewer\USART1</Name>
+          <WinId>35902</WinId>
+        </Entry>
+      </SystemViewers>
     </TargetOption>
   </Target>
 
@@ -339,18 +417,6 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\motor\encoder.c</PathWithFileName>
-      <FilenameWithoutPath>encoder.c</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
-    <File>
-      <GroupNumber>2</GroupNumber>
-      <FileNumber>11</FileNumber>
-      <FileType>1</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
       <PathWithFileName>..\Applications\foc\core\torque.c</PathWithFileName>
       <FilenameWithoutPath>torque.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
@@ -358,7 +424,7 @@
     </File>
     <File>
       <GroupNumber>2</GroupNumber>
-      <FileNumber>12</FileNumber>
+      <FileNumber>11</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -376,6 +442,18 @@
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
+    <File>
+      <GroupNumber>3</GroupNumber>
+      <FileNumber>12</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Applications\foc\motor\encoder.c</PathWithFileName>
+      <FilenameWithoutPath>encoder.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
     <File>
       <GroupNumber>3</GroupNumber>
       <FileNumber>13</FileNumber>

Project/MC100.uvprojx

@@ -433,11 +433,6 @@
               <FileType>1</FileType>
               <FilePath>..\Applications\foc\core\PMSM_FOC_Core.c</FilePath>
             </File>
-            <File>
-              <FileName>encoder.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\motor\encoder.c</FilePath>
-            </File>
             <File>
               <FileName>torque.c</FileName>
               <FileType>1</FileType>
@@ -453,6 +448,11 @@
         <Group>
           <GroupName>Motor</GroupName>
           <Files>
+            <File>
+              <FileName>encoder.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\foc\motor\encoder.c</FilePath>
+            </File>
             <File>
               <FileName>current.c</FileName>
               <FileType>1</FileType>

Simulink/Fir_angle.m

@@ -0,0 +1,26 @@
+function Hd = Fir_angle
+%FIR_ANGLE 返回离散时间滤波器对象。
+
+% MATLAB Code
+% Generated by MATLAB(R) 9.9 and DSP System Toolbox 9.11.
+% Generated on: 04-Oct-2022 16:35:03
+
+% Equiripple Lowpass filter designed using the FIRPM function.
+
+% All frequency values are in Hz.
+Fs = 500;  % Sampling Frequency
+
+Fpass = 0.5;               % Passband Frequency
+Fstop = 3;                 % Stopband Frequency
+Dpass = 5.7564627324e-06;  % Passband Ripple
+Dstop = 0.0001;            % Stopband Attenuation
+dens  = 20;                % Density Factor
+
+% Calculate the order from the parameters using FIRPMORD.
+[N, Fo, Ao, W] = firpmord([Fpass, Fstop]/(Fs/2), [1 0], [Dpass, Dstop]);
+
+% Calculate the coefficients using the FIRPM function.
+b  = firpm(N, Fo, Ao, W, {dens});
+Hd = dfilt.dffir(b);
+
+% [EOF]

Simulink/angle.csv

@@ -0,0 +1,3072 @@
+-289
+-309
+-250
+-170
+-140
+-181
+-134
+-41
+-31
+-73
+-115
+-112
+-102
+-131
+-147
+-153
+-148
+-203
+-209
+-168
+-153
+-246
+-282
+-225
+-244
+-302
+-324
+-320
+-320
+-395
+-410
+-408
+-423
+-476
+-454
+-365
+-333
+-333
+-302
+-234
+-182
+-183
+-173
+-131
+-133
+-174
+-204
+-197
+-197
+-260
+-316
+-298
+-312
+-330
+-349
+-320
+-322
+-354
+-346
+-309
+-329
+-353
+-377
+-326
+-329
+-365
+-387
+-359
+-321
+-311
+-292
+-230
+-183
+-169
+-131
+-79
+-19
+-39
+-57
+-66
+-38
+-74
+-76
+-79
+-81
+-98
+-98
+-46
+-37
+-64
+-65
+-52
+-52
+-55
+-83
+-62
+-91
+-139
+-160
+-151
+-174
+-199
+-199
+-153
+-112
+-106
+-96
+-30
+20
+38
+23
+61
+88
+39
+-24
+-17
+-59
+-85
+-162
+-187
+-200
+-220
+-247
+-233
+-250
+-266
+-273
+-236
+-226
+-256
+-284
+-272
+-228
+-273
+-295
+-307
+-300
+-300
+-276
+-212
+-161
+-164
+-144
+-48
+-33
+-32
+-23
+-27
+-45
+-49
+-72
+-32
+-78
+-96
+-110
+-78
+-42
+-87
+-85
+-71
+-67
+-124
+-125
+-99
+-149
+-177
+-192
+-149
+-202
+-243
+-239
+-186
+-157
+-152
+-125
+-55
+8
+-12
+27
+98
+124
+72
+79
+81
+86
+51
+23
+27
+25
+0
+-26
+38
+41
+5
+15
+63
+63
+17
+30
+44
+7
+-16
+-45
+-52
+-60
+-82
+-59
+7
+29
+24
+41
+130
+161
+137
+161
+205
+144
+112
+97
+138
+73
+53
+8
+42
+34
+-11
+-44
+-3
+-51
+-95
+-86
+-62
+-103
+-152
+-180
+-171
+-178
+-228
+-264
+-198
+-174
+-177
+-144
+-79
+-51
+-71
+-25
+59
+83
+0
+-23
+15
+10
+-39
+-82
+-74
+-130
+-171
+-176
+-154
+-201
+-221
+-223
+-191
+-176
+-231
+-265
+-211
+-211
+-245
+-244
+-228
+-259
+-304
+-251
+-168
+-151
+-156
+-133
+-43
+-3
+-43
+-23
+7
+-4
+-67
+-53
+-51
+-36
+-107
+-105
+-47
+-35
+-89
+-81
+-16
+2
+-77
+-53
+-2
+-50
+-103
+-129
+-73
+-101
+-163
+-164
+-110
+-114
+-106
+-87
+7
+29
+12
+28
+121
+132
+73
+58
+61
+63
+19
+-16
+-7
+-13
+-91
+-80
+-54
+-93
+-140
+-154
+-80
+-98
+-147
+-127
+-147
+-145
+-183
+-186
+-186
+-229
+-266
+-229
+-136
+-120
+-160
+-98
+-19
+8
+-12
+29
+70
+56
+19
+-5
+48
+47
+-34
+-49
+-40
+-34
+-95
+-84
+-75
+-63
+-99
+-107
+-75
+-108
+-144
+-183
+-175
+-195
+-281
+-286
+-237
+-237
+-248
+-190
+-115
+-96
+-115
+-52
+18
+51
+12
+16
+44
+21
+-20
+-33
+-14
+-42
+-95
+-90
+-38
+-47
+-95
+-72
+-40
+-33
+-55
+-37
+-44
+-45
+-71
+-80
+-42
+-84
+-120
+-101
+-26
+1
+-12
+33
+109
+137
+117
+158
+199
+168
+91
+92
+103
+109
+33
+12
+12
+35
+-28
+-34
+-19
+-40
+-76
+-83
+-36
+-104
+-142
+-158
+-157
+-182
+-215
+-255
+-219
+-200
+-232
+-189
+-104
+-88
+-97
+-46
+15
+42
+4
+-9
+5
+-7
+-74
+-101
+-97
+-131
+-195
+-220
+-191
+-208
+-248
+-257
+-223
+-199
+-239
+-235
+-215
+-213
+-256
+-225
+-169
+-223
+-260
+-223
+-158
+-136
+-154
+-85
+-21
+12
+11
+60
+119
+114
+63
+49
+112
+102
+80
+80
+118
+108
+90
+118
+169
+154
+94
+152
+184
+174
+119
+125
+117
+81
+53
+58
+57
+22
+12
+57
+112
+133
+140
+176
+232
+265
+258
+280
+317
+261
+201
+194
+168
+164
+142
+98
+107
+59
+7
+10
+20
+9
+-43
+-25
+-7
+-30
+-78
+-86
+-66
+-118
+-174
+-178
+-132
+-126
+-157
+-90
+-24
+2
+-8
+32
+101
+127
+99
+41
+97
+90
+31
+11
+18
+2
+-54
+-36
+-17
+-15
+-61
+-73
+-37
+-51
+-84
+-123
+-124
+-125
+-190
+-188
+-156
+-186
+-185
+-153
+-65
+-50
+-68
+-20
+68
+93
+87
+83
+120
+111
+63
+33
+63
+-4
+-57
+-41
+12
+20
+-43
+-26
+-3
+-2
+-43
+-33
+-7
+-17
+-70
+-56
+-24
+-55
+-131
+-113
+-58
+-41
+-47
+-20
+64
+89
+72
+107
+158
+163
+113
+58
+66
+67
+-5
+-48
+-49
+-55
+-96
+-126
+-104
+-124
+-162
+-163
+-144
+-145
+-192
+-204
+-215
+-234
+-256
+-277
+-277
+-279
+-305
+-252
+-190
+-174
+-155
+-109
+-60
+-28
+-40
+-30
+-25
+-46
+-93
+-84
+-93
+-136
+-197
+-188
+-174
+-211
+-220
+-232
+-213
+-210
+-225
+-218
+-210
+-247
+-280
+-255
+-227
+-295
+-313
+-286
+-256
+-247
+-216
+-163
+-122
+-116
+-82
+-16
+1
+-16
+-34
+-39
+-27
+-37
+-48
+-35
+-61
+-66
+-55
+-27
+-39
+-46
+-47
+-33
+-44
+-69
+-101
+-131
+-161
+-191
+-213
+-218
+-272
+-273
+-242
+-216
+-180
+-182
+-140
+-91
+-78
+-69
+-43
+-58
+-87
+-124
+-143
+-157
+-191
+-252
+-279
+-285
+-312
+-343
+-359
+-371
+-379
+-393
+-383
+-402
+-448
+-467
+-465
+-464
+-525
+-560
+-532
+-514
+-482
+-461
+-427
+-391
+-372
+-342
+-313
+-267
+-299
+-326
+-323
+-341
+-350
+-372
+-413
+-422
+-459
+-447
+-443
+-452
+-495
+-501
+-488
+-492
+-524
+-561
+-590
+-585
+-604
+-626
+-638
+-646
+-657
+-640
+-582
+-547
+-547
+-504
+-454
+-421
+-425
+-416
+-380
+-400
+-473
+-480
+-479
+-506
+-564
+-561
+-540
+-566
+-587
+-566
+-538
+-537
+-541
+-531
+-505
+-510
+-563
+-570
+-540
+-554
+-600
+-596
+-555
+-534
+-524
+-492
+-443
+-397
+-398
+-364
+-320
+-311
+-361
+-368
+-374
+-386
+-406
+-416
+-428
+-442
+-467
+-459
+-451
+-475
+-534
+-520
+-493
+-512
+-539
+-565
+-595
+-599
+-643
+-654
+-660
+-672
+-692
+-649
+-605
+-577
+-554
+-524
+-460
+-442
+-432
+-400
+-408
+-425
+-462
+-465
+-454
+-493
+-529
+-580
+-587
+-587
+-622
+-632
+-644
+-652
+-634
+-625
+-627
+-653
+-676
+-683
+-666
+-711
+-731
+-748
+-737
+-703
+-669
+-621
+-590
+-556
+-518
+-478
+-458
+-426
+-429
+-446
+-454
+-437
+-457
+-474
+-490
+-480
+-470
+-462
+-475
+-466
+-432
+-435
+-449
+-441
+-438
+-493
+-512
+-526
+-523
+-545
+-576
+-587
+-542
+-517
+-525
+-494
+-416
+-391
+-407
+-381
+-303
+-282
+-318
+-342
+-360
+-390
+-449
+-451
+-468
+-493
+-534
+-584
+-559
+-591
+-633
+-633
+-596
+-589
+-642
+-637
+-625
+-667
+-689
+-680
+-702
+-719
+-780
+-723
+-638
+-612
+-638
+-588
+-517
+-481
+-498
+-488
+-473
+-469
+-507
+-537
+-487
+-525
+-570
+-614
+-574
+-575
+-624
+-642
+-602
+-599
+-625
+-622
+-575
+-621
+-662
+-681
+-616
+-653
+-713
+-723
+-645
+-649
+-672
+-603
+-511
+-477
+-495
+-451
+-350
+-317
+-353
+-375
+-348
+-338
+-389
+-370
+-354
+-339
+-427
+-402
+-333
+-315
+-348
+-361
+-293
+-295
+-346
+-317
+-258
+-286
+-356
+-335
+-288
+-331
+-380
+-387
+-297
+-289
+-309
+-250
+-170
+-140
+-181
+-134
+-41
+-31
+-73
+-115
+-112
+-102
+-131
+-147
+-153
+-148
+-203
+-209
+-168
+-153
+-246
+-282
+-225
+-244
+-302
+-324
+-320
+-320
+-395
+-410
+-408
+-423
+-476
+-454
+-365
+-333
+-333
+-302
+-234
+-182
+-183
+-173
+-131
+-133
+-174
+-204
+-197
+-197
+-260
+-316
+-298
+-312
+-330
+-349
+-320
+-322
+-354
+-346
+-309
+-329
+-353
+-377
+-326
+-329
+-365
+-387
+-359
+-321
+-311
+-292
+-230
+-183
+-169
+-131
+-79
+-19
+-39
+-57
+-66
+-38
+-74
+-76
+-79
+-81
+-98
+-98
+-46
+-37
+-64
+-65
+-52
+-52
+-55
+-83
+-62
+-91
+-139
+-160
+-151
+-174
+-199
+-199
+-153
+-112
+-106
+-96
+-30
+20
+38
+23
+61
+88
+39
+-24
+-17
+-59
+-85
+-162
+-187
+-200
+-220
+-247
+-233
+-250
+-266
+-273
+-236
+-226
+-256
+-284
+-272
+-228
+-273
+-295
+-307
+-300
+-300
+-276
+-212
+-161
+-164
+-144
+-48
+-33
+-32
+-23
+-27
+-45
+-49
+-72
+-32
+-78
+-96
+-110
+-78
+-42
+-87
+-85
+-71
+-67
+-124
+-125
+-99
+-149
+-177
+-192
+-149
+-202
+-243
+-239
+-186
+-157
+-152
+-125
+-55
+8
+-12
+27
+98
+124
+72
+79
+81
+86
+51
+23
+27
+25
+0
+-26
+38
+41
+5
+15
+63
+63
+17
+30
+44
+7
+-16
+-45
+-52
+-60
+-82
+-59
+7
+29
+24
+41
+130
+161
+137
+161
+205
+144
+112
+97
+138
+73
+53
+8
+42
+34
+-11
+-44
+-3
+-51
+-95
+-86
+-62
+-103
+-152
+-180
+-171
+-178
+-228
+-264
+-198
+-174
+-177
+-144
+-79
+-51
+-71
+-25
+59
+83
+0
+-23
+15
+10
+-39
+-82
+-74
+-130
+-171
+-176
+-154
+-201
+-221
+-223
+-191
+-176
+-231
+-265
+-211
+-211
+-245
+-244
+-228
+-259
+-304
+-251
+-168
+-151
+-156
+-133
+-43
+-3
+-43
+-23
+7
+-4
+-67
+-53
+-51
+-36
+-107
+-105
+-47
+-35
+-89
+-81
+-16
+2
+-77
+-53
+-2
+-50
+-103
+-129
+-73
+-101
+-163
+-164
+-110
+-114
+-106
+-87
+7
+29
+12
+28
+121
+132
+73
+58
+61
+63
+19
+-16
+-7
+-13
+-91
+-80
+-54
+-93
+-140
+-154
+-80
+-98
+-147
+-127
+-147
+-145
+-183
+-186
+-186
+-229
+-266
+-229
+-136
+-120
+-160
+-98
+-19
+8
+-12
+29
+70
+56
+19
+-5
+48
+47
+-34
+-49
+-40
+-34
+-95
+-84
+-75
+-63
+-99
+-107
+-75
+-108
+-144
+-183
+-175
+-195
+-281
+-286
+-237
+-237
+-248
+-190
+-115
+-96
+-115
+-52
+18
+51
+12
+16
+44
+21
+-20
+-33
+-14
+-42
+-95
+-90
+-38
+-47
+-95
+-72
+-40
+-33
+-55
+-37
+-44
+-45
+-71
+-80
+-42
+-84
+-120
+-101
+-26
+1
+-12
+33
+109
+137
+117
+158
+199
+168
+91
+92
+103
+109
+33
+12
+12
+35
+-28
+-34
+-19
+-40
+-76
+-83
+-36
+-104
+-142
+-158
+-157
+-182
+-215
+-255
+-219
+-200
+-232
+-189
+-104
+-88
+-97
+-46
+15
+42
+4
+-9
+5
+-7
+-74
+-101
+-97
+-131
+-195
+-220
+-191
+-208
+-248
+-257
+-223
+-199
+-239
+-235
+-215
+-213
+-256
+-225
+-169
+-223
+-260
+-223
+-158
+-136
+-154
+-85
+-21
+12
+11
+60
+119
+114
+63
+49
+112
+102
+80
+80
+118
+108
+90
+118
+169
+154
+94
+152
+184
+174
+119
+125
+117
+81
+53
+58
+57
+22
+12
+57
+112
+133
+140
+176
+232
+265
+258
+280
+317
+261
+201
+194
+168
+164
+142
+98
+107
+59
+7
+10
+20
+9
+-43
+-25
+-7
+-30
+-78
+-86
+-66
+-118
+-174
+-178
+-132
+-126
+-157
+-90
+-24
+2
+-8
+32
+101
+127
+99
+41
+97
+90
+31
+11
+18
+2
+-54
+-36
+-17
+-15
+-61
+-73
+-37
+-51
+-84
+-123
+-124
+-125
+-190
+-188
+-156
+-186
+-185
+-153
+-65
+-50
+-68
+-20
+68
+93
+87
+83
+120
+111
+63
+33
+63
+-4
+-57
+-41
+12
+20
+-43
+-26
+-3
+-2
+-43
+-33
+-7
+-17
+-70
+-56
+-24
+-55
+-131
+-113
+-58
+-41
+-47
+-20
+64
+89
+72
+107
+158
+163
+113
+58
+66
+67
+-5
+-48
+-49
+-55
+-96
+-126
+-104
+-124
+-162
+-163
+-144
+-145
+-192
+-204
+-215
+-234
+-256
+-277
+-277
+-279
+-305
+-252
+-190
+-174
+-155
+-109
+-60
+-28
+-40
+-30
+-25
+-46
+-93
+-84
+-93
+-136
+-197
+-188
+-174
+-211
+-220
+-232
+-213
+-210
+-225
+-218
+-210
+-247
+-280
+-255
+-227
+-295
+-313
+-286
+-256
+-247
+-216
+-163
+-122
+-116
+-82
+-16
+1
+-16
+-34
+-39
+-27
+-37
+-48
+-35
+-61
+-66
+-55
+-27
+-39
+-46
+-47
+-33
+-44
+-69
+-101
+-131
+-161
+-191
+-213
+-218
+-272
+-273
+-242
+-216
+-180
+-182
+-140
+-91
+-78
+-69
+-43
+-58
+-87
+-124
+-143
+-157
+-191
+-252
+-279
+-285
+-312
+-343
+-359
+-371
+-379
+-393
+-383
+-402
+-448
+-467
+-465
+-464
+-525
+-560
+-532
+-514
+-482
+-461
+-427
+-391
+-372
+-342
+-313
+-267
+-299
+-326
+-323
+-341
+-350
+-372
+-413
+-422
+-459
+-447
+-443
+-452
+-495
+-501
+-488
+-492
+-524
+-561
+-590
+-585
+-604
+-626
+-638
+-646
+-657
+-640
+-582
+-547
+-547
+-504
+-454
+-421
+-425
+-416
+-380
+-400
+-473
+-480
+-479
+-506
+-564
+-561
+-540
+-566
+-587
+-566
+-538
+-537
+-541
+-531
+-505
+-510
+-563
+-570
+-540
+-554
+-600
+-596
+-555
+-534
+-524
+-492
+-443
+-397
+-398
+-364
+-320
+-311
+-361
+-368
+-374
+-386
+-406
+-416
+-428
+-442
+-467
+-459
+-451
+-475
+-534
+-520
+-493
+-512
+-539
+-565
+-595
+-599
+-643
+-654
+-660
+-672
+-692
+-649
+-605
+-577
+-554
+-524
+-460
+-442
+-432
+-400
+-408
+-425
+-462
+-465
+-454
+-493
+-529
+-580
+-587
+-587
+-622
+-632
+-644
+-652
+-634
+-625
+-627
+-653
+-676
+-683
+-666
+-711
+-731
+-748
+-737
+-703
+-669
+-621
+-590
+-556
+-518
+-478
+-458
+-426
+-429
+-446
+-454
+-437
+-457
+-474
+-490
+-480
+-470
+-462
+-475
+-466
+-432
+-435
+-449
+-441
+-438
+-493
+-512
+-526
+-523
+-545
+-576
+-587
+-542
+-517
+-525
+-494
+-416
+-391
+-407
+-381
+-303
+-282
+-318
+-342
+-360
+-390
+-449
+-451
+-468
+-493
+-534
+-584
+-559
+-591
+-633
+-633
+-596
+-589
+-642
+-637
+-625
+-667
+-689
+-680
+-702
+-719
+-780
+-723
+-638
+-612
+-638
+-588
+-517
+-481
+-498
+-488
+-473
+-469
+-507
+-537
+-487
+-525
+-570
+-614
+-574
+-575
+-624
+-642
+-602
+-599
+-625
+-622
+-575
+-621
+-662
+-681
+-616
+-653
+-713
+-723
+-645
+-649
+-672
+-603
+-511
+-477
+-495
+-451
+-350
+-317
+-353
+-375
+-348
+-338
+-389
+-370
+-354
+-339
+-427
+-402
+-333
+-315
+-348
+-361
+-293
+-295
+-346
+-317
+-258
+-286
+-356
+-335
+-288
+-331
+-380
+-387
+-297
+-289
+-309
+-250
+-170
+-140
+-181
+-134
+-41
+-31
+-73
+-115
+-112
+-102
+-131
+-147
+-153
+-148
+-203
+-209
+-168
+-153
+-246
+-282
+-225
+-244
+-302
+-324
+-320
+-320
+-395
+-410
+-408
+-423
+-476
+-454
+-365
+-333
+-333
+-302
+-234
+-182
+-183
+-173
+-131
+-133
+-174
+-204
+-197
+-197
+-260
+-316
+-298
+-312
+-330
+-349
+-320
+-322
+-354
+-346
+-309
+-329
+-353
+-377
+-326
+-329
+-365
+-387
+-359
+-321
+-311
+-292
+-230
+-183
+-169
+-131
+-79
+-19
+-39
+-57
+-66
+-38
+-74
+-76
+-79
+-81
+-98
+-98
+-46
+-37
+-64
+-65
+-52
+-52
+-55
+-83
+-62
+-91
+-139
+-160
+-151
+-174
+-199
+-199
+-153
+-112
+-106
+-96
+-30
+20
+38
+23
+61
+88
+39
+-24
+-17
+-59
+-85
+-162
+-187
+-200
+-220
+-247
+-233
+-250
+-266
+-273
+-236
+-226
+-256
+-284
+-272
+-228
+-273
+-295
+-307
+-300
+-300
+-276
+-212
+-161
+-164
+-144
+-48
+-33
+-32
+-23
+-27
+-45
+-49
+-72
+-32
+-78
+-96
+-110
+-78
+-42
+-87
+-85
+-71
+-67
+-124
+-125
+-99
+-149
+-177
+-192
+-149
+-202
+-243
+-239
+-186
+-157
+-152
+-125
+-55
+8
+-12
+27
+98
+124
+72
+79
+81
+86
+51
+23
+27
+25
+0
+-26
+38
+41
+5
+15
+63
+63
+17
+30
+44
+7
+-16
+-45
+-52
+-60
+-82
+-59
+7
+29
+24
+41
+130
+161
+137
+161
+205
+144
+112
+97
+138
+73
+53
+8
+42
+34
+-11
+-44
+-3
+-51
+-95
+-86
+-62
+-103
+-152
+-180
+-171
+-178
+-228
+-264
+-198
+-174
+-177
+-144
+-79
+-51
+-71
+-25
+59
+83
+0
+-23
+15
+10
+-39
+-82
+-74
+-130
+-171
+-176
+-154
+-201
+-221
+-223
+-191
+-176
+-231
+-265
+-211
+-211
+-245
+-244
+-228
+-259
+-304
+-251
+-168
+-151
+-156
+-133
+-43
+-3
+-43
+-23
+7
+-4
+-67
+-53
+-51
+-36
+-107
+-105
+-47
+-35
+-89
+-81
+-16
+2
+-77
+-53
+-2
+-50
+-103
+-129
+-73
+-101
+-163
+-164
+-110
+-114
+-106
+-87
+7
+29
+12
+28
+121
+132
+73
+58
+61
+63
+19
+-16
+-7
+-13
+-91
+-80
+-54
+-93
+-140
+-154
+-80
+-98
+-147
+-127
+-147
+-145
+-183
+-186
+-186
+-229
+-266
+-229
+-136
+-120
+-160
+-98
+-19
+8
+-12
+29
+70
+56
+19
+-5
+48
+47
+-34
+-49
+-40
+-34
+-95
+-84
+-75
+-63
+-99
+-107
+-75
+-108
+-144
+-183
+-175
+-195
+-281
+-286
+-237
+-237
+-248
+-190
+-115
+-96
+-115
+-52
+18
+51
+12
+16
+44
+21
+-20
+-33
+-14
+-42
+-95
+-90
+-38
+-47
+-95
+-72
+-40
+-33
+-55
+-37
+-44
+-45
+-71
+-80
+-42
+-84
+-120
+-101
+-26
+1
+-12
+33
+109
+137
+117
+158
+199
+168
+91
+92
+103
+109
+33
+12
+12
+35
+-28
+-34
+-19
+-40
+-76
+-83
+-36
+-104
+-142
+-158
+-157
+-182
+-215
+-255
+-219
+-200
+-232
+-189
+-104
+-88
+-97
+-46
+15
+42
+4
+-9
+5
+-7
+-74
+-101
+-97
+-131
+-195
+-220
+-191
+-208
+-248
+-257
+-223
+-199
+-239
+-235
+-215
+-213
+-256
+-225
+-169
+-223
+-260
+-223
+-158
+-136
+-154
+-85
+-21
+12
+11
+60
+119
+114
+63
+49
+112
+102
+80
+80
+118
+108
+90
+118
+169
+154
+94
+152
+184
+174
+119
+125
+117
+81
+53
+58
+57
+22
+12
+57
+112
+133
+140
+176
+232
+265
+258
+280
+317
+261
+201
+194
+168
+164
+142
+98
+107
+59
+7
+10
+20
+9
+-43
+-25
+-7
+-30
+-78
+-86
+-66
+-118
+-174
+-178
+-132
+-126
+-157
+-90
+-24
+2
+-8
+32
+101
+127
+99
+41
+97
+90
+31
+11
+18
+2
+-54
+-36
+-17
+-15
+-61
+-73
+-37
+-51
+-84
+-123
+-124
+-125
+-190
+-188
+-156
+-186
+-185
+-153
+-65
+-50
+-68
+-20
+68
+93
+87
+83
+120
+111
+63
+33
+63
+-4
+-57
+-41
+12
+20
+-43
+-26
+-3
+-2
+-43
+-33
+-7
+-17
+-70
+-56
+-24
+-55
+-131
+-113
+-58
+-41
+-47
+-20
+64
+89
+72
+107
+158
+163
+113
+58
+66
+67
+-5
+-48
+-49
+-55
+-96
+-126
+-104
+-124
+-162
+-163
+-144
+-145
+-192
+-204
+-215
+-234
+-256
+-277
+-277
+-279
+-305
+-252
+-190
+-174
+-155
+-109
+-60
+-28
+-40
+-30
+-25
+-46
+-93
+-84
+-93
+-136
+-197
+-188
+-174
+-211
+-220
+-232
+-213
+-210
+-225
+-218
+-210
+-247
+-280
+-255
+-227
+-295
+-313
+-286
+-256
+-247
+-216
+-163
+-122
+-116
+-82
+-16
+1
+-16
+-34
+-39
+-27
+-37
+-48
+-35
+-61
+-66
+-55
+-27
+-39
+-46
+-47
+-33
+-44
+-69
+-101
+-131
+-161
+-191
+-213
+-218
+-272
+-273
+-242
+-216
+-180
+-182
+-140
+-91
+-78
+-69
+-43
+-58
+-87
+-124
+-143
+-157
+-191
+-252
+-279
+-285
+-312
+-343
+-359
+-371
+-379
+-393
+-383
+-402
+-448
+-467
+-465
+-464
+-525
+-560
+-532
+-514
+-482
+-461
+-427
+-391
+-372
+-342
+-313
+-267
+-299
+-326
+-323
+-341
+-350
+-372
+-413
+-422
+-459
+-447
+-443
+-452
+-495
+-501
+-488
+-492
+-524
+-561
+-590
+-585
+-604
+-626
+-638
+-646
+-657
+-640
+-582
+-547
+-547
+-504
+-454
+-421
+-425
+-416
+-380
+-400
+-473
+-480
+-479
+-506
+-564
+-561
+-540
+-566
+-587
+-566
+-538
+-537
+-541
+-531
+-505
+-510
+-563
+-570
+-540
+-554
+-600
+-596
+-555
+-534
+-524
+-492
+-443
+-397
+-398
+-364
+-320
+-311
+-361
+-368
+-374
+-386
+-406
+-416
+-428
+-442
+-467
+-459
+-451
+-475
+-534
+-520
+-493
+-512
+-539
+-565
+-595
+-599
+-643
+-654
+-660
+-672
+-692
+-649
+-605
+-577
+-554
+-524
+-460
+-442
+-432
+-400
+-408
+-425
+-462
+-465
+-454
+-493
+-529
+-580
+-587
+-587
+-622
+-632
+-644
+-652
+-634
+-625
+-627
+-653
+-676
+-683
+-666
+-711
+-731
+-748
+-737
+-703
+-669
+-621
+-590
+-556
+-518
+-478
+-458
+-426
+-429
+-446
+-454
+-437
+-457
+-474
+-490
+-480
+-470
+-462
+-475
+-466
+-432
+-435
+-449
+-441
+-438
+-493
+-512
+-526
+-523
+-545
+-576
+-587
+-542
+-517
+-525
+-494
+-416
+-391
+-407
+-381
+-303
+-282
+-318
+-342
+-360
+-390
+-449
+-451
+-468
+-493
+-534
+-584
+-559
+-591
+-633
+-633
+-596
+-589
+-642
+-637
+-625
+-667
+-689
+-680
+-702
+-719
+-780
+-723
+-638
+-612
+-638
+-588
+-517
+-481
+-498
+-488
+-473
+-469
+-507
+-537
+-487
+-525
+-570
+-614
+-574
+-575
+-624
+-642
+-602
+-599
+-625
+-622
+-575
+-621
+-662
+-681
+-616
+-653
+-713
+-723
+-645
+-649
+-672
+-603
+-511
+-477
+-495
+-451
+-350
+-317
+-353
+-375
+-348
+-338
+-389
+-370
+-354
+-339
+-427
+-402
+-333
+-315
+-348
+-361
+-293
+-295
+-346
+-317
+-258
+-286
+-356
+-335
+-288
+-331
+-380
+-387
+-297