2FOC current filter implemented.

[ale-git]

• Since PWM and current feedback are transmitted at 1 kHz, but set/measured at 20 kHz, I've introduced a filter acting in a 20 sample wide window.
• The PWM 1 kHz feedback is the average of 20 values in a 1 msec window.
• The current 1 kHz feedback is the average of 14 samples in a 1 msec window because the 3 highest and the 3 lowest values are discarded.
• Moreover, the current signal for current control is the average of two consecutive samples.
• The ADC is calibrated at startup (offset and gain).
• I've removed the assembly routines for current calibration because anachronistic and difficult to maintain, and replaced with two lines of C code.

[valegagge]

Hi @ale-git,
can clarify that means The ADC is calibrated at startup (offset and gain). ? does the ADC calibration start after the reception of first motor enable command and not during the configuration in order to avoid to enable motors when thejoint are in a not-Configured state?

When this PR is ready can you please explain briefly the tests you did?
I saw there are some ongoing tests with AMI group.

thanks heaps

[ale-git]

Hi @valegagge, the offset calibration is made at startup since the motors are (and must be) completely OFF in this phase. In practice, you register the current measured by the sensor when the real current is 0 for sure, and you take this value as sensor offset.
The gain instead is measured with a small PWM applied, an thus it is performed ad first motor enable so that we don't have unexpected motion. It is not possible to measure the absolute gain because a third external reference would be necessary, but we balance in this way the two A and C channels (channel B has no current sensor since Ib = -(Ia+Ic)), by applying a +4% max_pwm to A and C and -8% max_pwm to B and measuring Ia and Ic, and then adjusting the gains Ka and Kc so that KaIa = KcIc with Ka*Kc = 1.
Now I'm going to put here the same plots of the experiments that I've put in the sprint private issue.

[ale-git]

I've operated the small ergoCub joint at constant PWM, free running, at 10% and 15% respectively (the speed increases significantly with the additional 5%). I've recorded the currents running the 3.3.36 version (unfiltered) and the 3.3.37 version (current filter). I've filtered the signals to extract the moving average in a 500 ms window, and then calculated the RMSE between the moving average and the raw measure.

[ale-git]

[marcoaccame]

Hi @ale-git, it seems fine by me.

[valegagge]

Hi @ale-git ,
could you please add some comments to the code to make it more readable?

Thanks! :)

[ale-git]

Hi @ale-git , could you please add some comments to the code to make it more readable?

Thanks! :)

Done!

[marcoaccame]

Hi @ale-git , could you please add some comments to the code to make it more readable?
Thanks! :)

Done!

Hi @valegagge, is the change ok? Shall I merge?

[marcoaccame]

Hi, I will merge so that we can clean up the open PRs. In case, we may produce further code comments in a later PR.

Linked PR:

• 2FOC v3.3.37 icub-firmware-build#170