|
|
@@ -290,12 +290,12 @@ float encoder_zero_phase_detect(float *enc_off) {
|
|
|
delay_ms(5);
|
|
|
float total_enc_off = g_encoder.pwm_count;
|
|
|
float prev_offset = g_encoder.enc_offset;
|
|
|
- float phase = g_encoder.pwm_angle;
|
|
|
+ float phase = encoder_get_pwm_angle();
|
|
|
float total_ph = phase;
|
|
|
int count = 0;
|
|
|
for(; count < 10; count++) {
|
|
|
delay_ms(5); //wait time for pwm
|
|
|
- float angle_now = g_encoder.pwm_angle;
|
|
|
+ float angle_now = encoder_get_pwm_angle();
|
|
|
if (ABS(phase - angle_now) > 2.0f) {
|
|
|
g_encoder.enc_offset = prev_offset;
|
|
|
g_encoder.enc_count_off = 0xFFFFFFFF;
|
|
|
@@ -303,15 +303,15 @@ float encoder_zero_phase_detect(float *enc_off) {
|
|
|
sys_debug("err %f, %f, %d\n", phase, angle_now, count);
|
|
|
return INVALID_ANGLE;
|
|
|
}
|
|
|
-
|
|
|
+
|
|
|
phase = angle_now;
|
|
|
total_ph += phase;
|
|
|
total_enc_off += g_encoder.pwm_count;
|
|
|
}
|
|
|
- sys_debug("count = %d\n", count);
|
|
|
- float offset_now = total_ph/(float)(count);
|
|
|
+ sys_debug("count = %d, %f, %d\n", count, total_enc_off, g_encoder.pwm_count);
|
|
|
+ float offset_now = total_ph/(float)(count + 1);
|
|
|
g_encoder.enc_offset = offset_now;
|
|
|
- g_encoder.enc_count_off = (u32)(total_enc_off/(float)(count));
|
|
|
+ g_encoder.enc_count_off = (u32)(total_enc_off/(float)(count + 1));
|
|
|
if (enc_off) {
|
|
|
*enc_off = (float)g_encoder.enc_count_off;
|
|
|
sys_debug("encoder off %f\n", *enc_off);
|