robotology / icub-firmware-models

Models underlying the iCub Firmware

iCub Firmware Models

This repository hosts all the models underlying the iCub Firmware.

• 🌐 Browse our online documentation. 🚧 Work in progress 🚧

Maintainers

This repository is maintained by:

	@pattacini

Contributing

We accept contributions via forks and pull-requests.

icub-firmware-models's Issues

AMCBLDC – Model the `i2t` overcurrent protection

We're required to implement in the architectural model the i2t protection.
We may use some new parameters as per #8.

Some further resources:

• https://webhelp.kollmorgen.com/kas/Content/UsersManual/Motor%20I2t%20algorithm.htm
• https://www.microprivod.ru/library/Tech_info/copley_app_notes/IsqT.pdf

AMCBLDC – Implement Optical Encoder Dirty

Probably, this is not a good candidate task to be implemented in Simulink as therein we assume that we already receive the elaborated position.

Nonetheless, we should integrate somehow (maybe in the hand-coded FW) what done by @ale-git in https://github.com/robotology/icub-firmware/blob/master/emBODY/eBcode/arch-dspic/board/2foc/appl/2FOC-V3/src/qep.c#L150-L198.

Update Wrist-Mk2 model – Let the controller deal with left/right + old/new mech

Following up on #45, we're required to update the control model to deal with:

• The left/right wrists.
• The old/new mechanics.

👉🏻📖 Once done, we need to update https://icub-tech-iit.github.io/documentation/wrists/wrist_mk2/?h=wrist.

cc @ale-git @salvi-mattia @valegagge @fiorisi @mfussi66 @Nicogene @marcoaccame

Extend data dictionary to deal with multiple motors – Stint 2

Follow-up of #54.

• Investigate how the components of the dictionary can be vectorized in view of the AMCBLDC refactoring
• Start off generating the code and see if there are critical aspects

Automatize performance analysis of codegen

From @valegagge:

I'd like to suggest improving the code inside the application in order to automatize as much as possible this check of performance: for example, we could have a simple Matlab script that could get the data collected in the firmware and plot them automatically. 🖌️

Run a test campaign before merging AMCBLDC_experimental and AMCLBDC

At the moment there are two projects that involve the AMCBLDC board, the official one and the Experimental one.
The Experimental project involves a major refactor of the supervisor and leverages the Common Simulink project to make the structure more modular.

While the first trials were successful (#84), the integration tests were not exhaustive.

This issue aims to track which tests need to be performed, before making Experimental the official project.

AMCBLDC: supervisorTX doesn't handle the overcurrent flag.

While working on issue #40, I noticed that the SupervisorTX supervisorTX doesn't handle the overcurrent flag, while the SupervisorRX performs the check.

SupervisorTX	SupervisorRX
As you can see, in the dictionary, Flafs struct doesn't contain the overcurrentinfo	The Fault handler state machine checks the filtered current value

I suppose this is a bug, isn't it? @pattacini @mfussi66

I can fix it before addressing the #40.

Extend data dictionary to deal with multiple motors – Stint 1

When #53 is merged, we need to make the dictionary general enough to contain structures dealing w/ 2 (or even more) motors.

Study the gut of the Supervisor

Get accustomed to all the details of the supervisor.

AMCFOC – Check resources for running 2 FOC algorithm inside CM4

As per the title, we aim to check whether there are limitations in running 2 MBSE FW inside the CM4 as we need to control 2 PMSM motors.

Update the CAN protocol doc

It's important to update the CAN protocol so that it'll contain all the missing packets.

Also, we would need to link to it from this website's documentation.

AMCBLDC: Inspect suspect regression in simulation

While working on AMCFOC Testing I tried to run a motor control simulation, using the latest devel branch.

To my surprise, the simulated pmsm could not spin because there was no commutation between phases:

We can see that 3-phase currents and voltages remain somewhat constant, while they actually should have this kind of waveform:

Closed loop current control:

Open loop voltage control:

So I checked the AMCBLDC project, to verify if upstream everything was working fine.
Most specifically I used the testing model https://github.com/robotology/icub-firmware-models/blob/devel/boards/amcbldc/tests/control/control_pmsm.slx , and I noticed the same behaviour happening starting from the PR regarding D-axis alignment, even though the physical motor spins with no problem.

I can make some tests to see which changes in this PR affect the simulation.

cc @Nicogene @pattacini @sgiraz

Perform preliminary firmware tests on the AMC-BLDC

Task description 📝

We recently refactored the motor control supervisor (see #65). We now have to test the new architecture on the AMC-BLDC board.
For this we need:

• to restructure the AMC-BLDC architectural model with the new supervisor
• a setup with a motor

Useful links and documents

Definition of Done ✅

Demo with setup running with the new firmware.

---

Originally opened by @fiorisi in different repo.

---

As suggested by @pattacini, it will be useful to create the AMCBLDC customizations in a separate path, so that the two versions can live side by side.

AMCFOC – Develop Architectural Model

This is the main collector for issues on the development of the architectural model for the AMCFOC board.

We will mainly inherit the model from the AMCBLDC and adapt it to a different context.
Some preliminary work on this includes:

• Extract general-purpose components from AMCBLDC and put them in a common basket.
• Check if 2 FOC components can run seamlessly on a single CM4 core.
• Study how to make the AMCBLDC/Supervisor general enough to cope with 2 motors.
• Get rid of CAN-based components as we'll rely on ETH ❔
  • We decided to do so!

Note
🗓️ Regarding the timeline, we ought to complete the development by the end of PI16 (~ Oct 2023) to be ready for testing the new ergoCubv2.0 arm at the end of the year.

Check direct current

We're required to verify that the direct current $I_d$ is kept to $0$ effectively by the FOC PID.
To this aim, we may rely on a temporary hack for the CAN protocol.
To be checked on the setup.

(Aside from $I_d$, it might be convenient to check the bare three-phase currents too.)

cc @Nicogene

AMCFOC – Refactor the Supervisor motor-independent part

Following up on the excellent work done on the dictionary (see #54), we need to deal with the motor-independent part of the Supervisor.

Important

It's true that we aim to address the AMCFOC, but it's essential to keep in mind that at a certain time we'll have also to integrate the AMCBLDC CAN handler and 4 motors (AMC4DC) instead of 2.

AMCFOC – Study how to make the AMCBLDC/Supervisor general enough to cope with 2 motors

This activity will be mainly performed by Team FIVE but @mfussi66 will be contributing as well.

@mfussi66 will be the main developer.

AMCBLDC – Make `Vmax` + other useful params configurable via CAN

As of now, Vcc and Vmax are statically defined in the model. This prevents us from adapting those parameters to the setups/robots w/o re-generating the code.

It'd be thus great to have the possibility to tune the voltage values on the fly via CAN, for example to adapt for cable or battery voltage supply.
We haven't done that yet as the current CAN protocol doesn't foresee this option.

EDIT

We may consider at this stage including other motor-related useful parameters to configure via CAN; see also #14.

In total:

• The Vmax.
• The motor resistance and inductance.
• Some control-specific params: swapBC, hall sensor offset.
• The thermal resistance, the thermal time constant, and the critical temperature.
• The environment temperature (perhaps).

AMCBLDC – Complete the `STATUS` msg

As of now, we only send the quadrature voltage and a flag reporting the status of the external e-stop.
However, the STATUS message has many more flags that are meant to be transmitted, although many are not yet handled internally.

Study and sketch a first proposal for extending the Supervisor_RX – Stint 1

In view of AMC-FOC, we need to extend Supervisor_RX to deal w/ 2 motors.

To this end, it would be nice to have some proposals to discuss highlighting the major parts needing overhaul.

AMCFOC – Port latest dev from AMCBLDC project

This is a follow-up of #73 to address these points below:

• Vcc is now an input.
• CMSIS vs. STD variant (it's ok to keep the variant, eventually).
• Variant for the Thermal Model.

cc @Nicogene

The switch Voltage → Current control is NOT bumpless

          Indeed, it seems that it happens also with the upstream architectural model, see the video.

Given that this workflow is very unlikely to happen, it might be useful to notify it in a separate issue with low priority.

MATLABWindow_hnlp1F7PTL.mp4

Originally posted by @mfussi66 in #84 (comment)

AMC-BLDC: the supervisor needs to manage the motor-hal errors

Following the work done here robotology/icub-firmware#360 (comment), we want to update the supervisor in order to manage the motor-hal errors: hardware-overcurrent and hall-sequence-error.

Dod

1. The supervisor is updated
2. The code has been generated
3. The code has been tested.

Note: the test is not simple because this error are hw-related, so we need to find a way to simulate them.

AMCBLDC – Read `Vcc` on the fly

It's be super useful to have the AMCBLDC board capable of reading the supply voltage Vcc on the fly in order to adapt wrt. changing conditions (network supply, batter, setup...).

To this end, we're required to develop a dedicated API and expose the value to the architectural model.
See also #8.

Publish and document the model and its codegen for coupling/decoupling ergoCub wrist.

Following the creation of the wrist controller and its integration in the ems fw, we want to publish and document the model and its generated fw for coupling/decoupling of eCub wrist.

Please see also this integration activity.

Refactor AMCBLDC project to rely on external components

Move the controllers and the estimators to a higher-level project.

AMCFOC: refactor motor-specific supervisor to be used with `For Each subsystem`

The idea of the AMCFOC motion controller is to be referenced in a For Each subsystem. This kind of subsystem allows calling the block multiple times, depending on the dimensionality of the input/output signals. In this way, we can use the same functions for multiple motors.
However, this type of subsystem relies on the internal function-call subsystems to have the trigger signal of width=1. The motor-specific supervisor is full of these triggers. It is therefore necessary to replace this kind of logic with an equivalent one, most likely with a less modular state machine, that allows us to resolve this limitation and enable the full features of the supervisor.
After this, it will be possible to create unit tests for the motor-specific supervisor.

Inspect and resolve the interaction between `For each subsystem` and FOCInnerLoop

The FOCInnerLoop block is the one called by the ADC interrupt in the firmware. Since the for-each subsystem expect to call this function once for each motor every ~36usec, we need to check if there are any implementation mismatches between the codegen and the expected behaviour which is interrupt based and not periodic.

AMCBLDC – Refactor the IRQ handler responsible for calling FOC

From @marcoaccame

---

Another thought:

I am a little nervous to have an important high priority and high frequency IRQ Handler that executes for long time. The rule of thumb is: the more important the IRQ handler, the less code it must run. Now: it is executed every 37 us, lasts about 15 us. Also it runs code which requires mutual exclusion. A little too much in my opinion.

I would clean up and move out of the IRQ Handler all the MBD code by doing the following.

• keep inside the IRQ Handler only a quick copy from DMA area of the three currents (maybe into a circular FIFO) and use an RTOS event to wake up a very high priority thread called threadFOC.
• the high priority thread threadFOC is waken up by an event from the IRQ handler which: copies just the three currents in a thread safe and very quick mode (I may do it perhaps also w/out disabling the IRQ...) and then use the three currents to run the MBD FOC algorithm.
• redefine the mutex used by the MBD code to be an RTOS mutex because it must protect data between two RTOS threads. The advantage is that the RTOS mutex has priority inversion.
• re-evaluate the duration and frequency of the IRQ Handler and of the two threads FOC and TICK.
• if the IRQ Handler is still not regular, I would check the priorities of the IRQ handlers and of the DMA to make sure no other has higher priority.

---

The framework of the application is a little messy. It was developed long ago and it needs maybe some tidy up.

For now I document in here what we need to have.

Figure. Scheme of the required SW entities.

We need a new thread tFOC alongside to the existing threads tCAN and tTICK. Also we need a different mutual exclusion mechanism between actions performed inside tTICK and tFOC: we can use a embot::os::rtos::mutex_t.

The interactions amongst entities are the following:

• tCAN -> object: when we have a new can frame from the embot::hw::can driver then thread tCAN, the producer, adds it to the input FIFO which is protected w/ a mutex vs concurrent access from tTICK, the consumer.
• tTICK -> tCAN: if there is a can frame to send back as a reply or a periodic streaming the thread tTICK sends an event to thread tCAN which retrieves a vector of replies from the output FIFO (mutex protected).
• tTICK <-> tFOC: access to shared data is done by means of a rtos::mutex_t.
• IRQ handler -> tFOC: the handler every 37 us has three new current values, it copies them into a volatile variable and then sends a wake up rtos event to thread tFOC. The thread wakes up, disable IRQ for the handler, copies the volatile variable into local memory and re-enables IRQ. At this time it runs safely the FOC algorithm.

The tTICK thread is periodic @ 1ms, the tFOC is activated by the periodic IRQ handler @ 37us (so the thread is de-facto periodic as well).

Almost all objects are available. I just need to refactor and test.

AMCBLDC – Implement the encoder index calibration for hall sensorless motors

In the AMCBLDC firmware there isn't an initial calibration procedure to find the index of the optical encoder. A similar procedure is already available on the 2FOC firmware.
The procedure could be implemented in the model, not in the HAL layer.

AMCFOC – Extend to AMCFOC the improvements recently applied to AMCBLDC

#64 introduced the improvements detailed in #64 (comment).

We ought to apply them to AMCFOC too.

@mfussi66, I can point you to the relevant options.

cc @Nicogene

AMCBLDC – Validate the thermal model further and link it to the control logic

Following up on #14, we're required to:

• Make more observations in a wider range of temperatures for validation purposes.
• Close the loop between the output of the thermal model and the control logic.

AMCBLDC – Complete the online documentation

So far, we have just published the webview of the architectural model at https://robotology.github.io/icub-firmware-models.

AMCFOC – Streamline block duplication

In #54 (comment), @mfussi66 found out that the For-Each approach is limited and cannot be applied straight to our case.

It'd be advantageous to poke around to see whether we can resort to methods better than our present solution based on block duplication.

Setting FOC Ts = 45us creates current oscillations

Setting the FOC Ts to 45us (instead of 37.5us , ndr) makes the motor spin with more oscillations, is this expected?

Originally posted by @mfussi66 in #84 (comment)

AMCFOC – Test the architecture

Following up on #60 (comment), we're required to:

• restore the custom code calls
• create proper automated test to verify if all conditions turn out as expected

Refine thermal model with inductive load

          A F2F chat with @pattacini and @maggia80 highlighted that it is important to consider how the PWM signal is generated (whether it is in Left-aligned mode or Center-aligned), and how the current is measured.

Since we are dealing with an inductive load, we need to consider exponential transitory effect, during which the PWM signal of a phase is positive.
The current is measured in the middle of the positive PWM, during which the transitory might not be exhausted. This depends on the time constant (Phase resistance and inductance), and on the duty cycle.

Therefore accounting for the electrical transitory period might be the key to improve the model and reduce the performance discrepancy in continuous current (aka Iq in the case above).

Support material:

• UNDERSTANDING THE EFFECT OF PWM WHEN CONTROLLING A BRUSHLESS DC MOTOR

Originally posted by @mfussi66 in #36 (comment)

Fit the thermal model

          Let's proceed with identifying the parameters $R_t$ and $\tau$ robustly by covering a wide range of operating conditions with currents and temperatures (it'll certainly take a while to do so).

As usual, the validation will need to be performed on motors/setup/operating-conditions not seen during fitting.

Originally posted by @pattacini in #34 (comment)

Study Service I/F to streamline mutexing

It could be beneficial to rethink the mutexing used in the generated code by resorting to Service I/F.
Watch https://www.youtube.com/watch?v=Y-6JvmTq21w.

Explore ways to support code reusability

A few possibilities to experiment with:

1. https://www.mathworks.com/help/ecoder/ug/generate-reusable-code-from-library-subsystems-shared-across-models.html
2. https://www.mathworks.com/help/rtw/ug/reusing-functions-in-generated-code.html

Use `Simulink Test` to automatize regression tests

Simulink Test is a blockset specifically devoted to testing.

Since we have already developed a bunch of nice tools for communicating with the AMCBLDC board, Simulink Test will seamlessly integrate with them to carry out preliminary regression tests like:

• changing modes
• current control
• logic control (e.g., the SW Current Limiter)

Also, we can produce nice reporting.

There's no hurry but we should definitely address it.

Here below some nice webinars we can inspect illustrating the perks of using a test harness and HIL:

• https://www.mathworks.com/videos/hardware-in-the-loop-hil-testing-of-an-electric-motor-controller-1606757339599.html
• https://www.speedgoat.com/solutions/testing-workflows/hardware-in-the-loop

Refactor AMCBLDC prj to expose library components

We aim to extract general-purpose components from the AMCBLDC project and put them in a shared basket/library to support other boards' FW.

Warning
We shall guarantee that AMCBLDC will remain 100% operational throughout the process with continuous testing.

Add documentation regarding code generation guidelines - Stint 1

Now that we have achieved some sort of expertise regarding code generation for an embedded board, it might be nice to create a .md document that highlights the best practices and settings to apply for an effective and flexible process.

For example, it could contain:

• Simulink model settings
• CMSIS settings
• Best practices for Simulink blocks
• Stateflow guidelines
• a sprinkle of dictionary usage

CMSIS routines for `sin`/`cos` are not used in FOC

While I was looking at the generated code of the FOC in the AMCBLDC, I spotted that the trigonometric functions for sin and cos are implemented by resorting to std instead of CMSIS.

I spent quite some time investigating this issue and, from the code generation report, it seems that the replacement library for the Cortex-M correctly detects the presence of sin and cos although it doesn't apply the substitution.

To check the performance gain, we can simply replace std::sin and std::cos with arm_sin_f32 and arm_cos_f32. However, the problem still remains when it comes to code generation.

There are easy alternatives that recruit proper approximations of sin and cos but the reason why the link to CMSIS is not performed is unclear.

Definitely, worth investigating.

Analyze FOC timing of AMCBLDC and AMC2C

          Let me try to recap what I got from a F2F alignment with @sgiraz.

1. There's pretty much NO difference between using STD or CMSIS ops on AMCBLDC and AMC2C.
2. On the AMCBLDC, the FOC is taking up $8$ $\mu s$ and NOT $17$ $\mu s$. The difference is represented by the ADC operations.
3. FOC takes up $14$ $\mu s$ on AMC2C, longer than on AMCBLDC.

• Point 1 completes this issue ✅ We go with STD. See #75.
• Follow-up question on Point 2: "Why does ADC last that long compared to FOC? Is there any margin for improvement?"
• Follow-up question on Point 3: "Why is AMC2C slower than AMCBLDC?" We know this is an ongoing study and we're probably at the right point to contact ST.

cc @Nicogene @sgiraz @marcoaccame @maggia80 @mfussi66

Originally posted by @pattacini in #67 (comment)

Update Wrist-Mk2 model – Physical components

We're required to update the Simscape model of wrist-mk2 by exporting the latest mechanics from CREO.

cc @ale-git @salvi-mattia @valegagge @fiorisi @mfussi66 @Nicogene