Add 2-way communication

oliverlee commented 6 years ago

We've talked about this for a while and want to implement 2-way communication to fix a number of issues which include:

Restarting the simulation without a hard reset. Thus we could restart without the motor receiving a high torque command due to poor decision in wiring and motor configuration.
Setting the simulator parameters. Change the bicycle model velocity (if fixed), change between fixed and non-fixed velocity, change bicycle model parameters, change controller parameters, etc.
Querying the simulator parameters. This will allow us to confirm what settings we are using.

This would involve defining messages as described in the following PRs: https://github.com/oliverlee/phobos/issues/181, https://github.com/oliverlee/phobos/issues/153, https://github.com/oliverlee/phobos/issues/235

I imagine we would need to introduce a simple state machine with the following states:

INIT: Program entry and sensor and actuator initialization. We would want to initialize and start the motor here and command a torque of zero so that we could restart the simulation without motor problems.

on entry: noop
on exit: noop
transitions:
- to STOP automatically

STOP: Reset all state associated with model and controller.

on entry: transmit COMMAND.STOP message
on exit: noop
transitions:
- to CONFIG on receipt of COMMAND.RUN message

CONFIG: Configure model and controller parameters.

on entry: transmit DATA.CONFIG message (with current configuration)
on exit: noop
transitions:
- to CONFIG on receipt of DATA.CONFIG message (after processing DATA.CONFIG and setting relevant parameters)
- to RUN on receipt of COMMAND.RUN
- to STOP on receipt of COMMAND.STOP

RUN: Run simulation.

on entry: enable sensors: necessary preparation for simulation which is not included in starting the peripheral (e.g. zero steer encoder).
on exit: disable actuators: necessary steps for cleanup of stopping simulation (e.g. setting motor to zero torque).
transitions:
- to STOP on receipt of COMMAND.STOP

For message, we differentiate between COMMAND and DATA types where a COMMAND message is simply a tag and a DATA message is a tag followed by an object as described in https://github.com/oliverlee/phobos/issues/181#issuecomment-301825244.

In order to fully implement this, I expect we need the following PRs:

[ ] Create a Receiver class
[ ] Define new protobuf messages and modify existing messages
[ ] Transmit different message types

mickvangelderen commented 6 years ago

Why are STOP and CONFIG different states?
Can the velocity be changed on the fly with this setup? Should we differentiate between parameters that cannot be changed in real time and ones that can?
Can we name the states more like states instead of commands, like INITIALIZING, STOPPED, RUNNING

oliverlee commented 6 years ago

Why are STOP and CONFIG different states?

I think you're right and they don't need to be.

Can the velocity be changed on the fly with this setup? Should we differentiate between parameters that cannot be changed in real time and ones that can?

Yes maybe we should have a config message for things that can be updated and things that cannot. At this time, I think all the fields that can be set can be changed in real time.

Can we name the states more like states instead of commands, like INITIALIZING, STOPPED, RUNNING

👍

mickvangelderen commented 6 years ago

Protobuf rework

Definitions

MC: The embedded microcontroller. PC: The pc to which the microcontroller is connected.

Code architecture

Add a receive thread to the MC. This thread is kept as dumb as possible. It is responsible for deserializing protobuf messages and enqueueing them for the simulation thread. It ignores deserialization errors.

The simulation thread will, while there is time left before the next deadline, dequeue messages that represent commands. These commands can stop or start the simulation or change certain parameters. when parameters are changed, an acknowledgement message must be enqueued for the transmitter thread containing a timestamp and the changed parameters. Large changes in parameters might have undesirable physical consequences. For now it is the responsibility of the PC to gradually change these parameters. The MC does have to check for nonsensical values.

These acknowledgement messages are also used during initialization to inform the PC of the initial configuration.

Message design

Configuration commands are defined for each set of configurable variables that must be changed all at once. This makes it easier to ensure sensible values are used in every simulation iteration. If values can be changed independently they should be changeable through different commands. This keeps the system flexible and the message size down which helps when defining memory pools.

The following observations motivate the creation of two separate memory pools to allocate messages from:

Memory is allocated statically.
When using oneof, the size of the largest message is used.
Simulation iteration data messages are ~800 bytes and sent every iteration.
Pose and command messages are ~80 bytes or less and sent in bursts.

We need several grouping oneof messages: (A) 1x PC to receive thread to simulation thread, 2x (B, C) simulation thread to transmit thread. (D) 1x transmit thread to PC.

The simplest approach to combine B, C would be to nest them inside D. Instead we embed the variants of B and C directly into D. Extending oneof is not supported but this might allow us to do memcpy CHECK THIS. Perhaps it is possible to handwrite part of the encoding so we can serialize directly from the memory pool objects CHECK THIS.

TODO: Revisit optional fields. Thought it would be nice to have everything optional so that the steerbywire could also be connected to the visualizer and just show what ever values are present.

oliverlee commented 6 years ago

Maximum encoded message sizes (079c530) pbSimulation_size: 400 pbPose_size: 35 pbBuild_size: 29 pbModelCanonical_size: (24 + 4*pbSecondOrderMatrix_size) = 104

/* pbRxMaster_size depends on runtime parameters */
/* pbSmallMessageGroup_size depends on runtime parameters */
/* pbTxMaster_size depends on runtime parameters */

Extending oneof is not supported but this might allow us to do memcpy CHECK THIS

Yes this should be possible due to the memory layout.

oliverlee commented 6 years ago

Here are the other sizes after fixing nanopb generation

#define pbEmpty_size                             0
#define pbModelCanonical_size                    88
#define pbFeedbackGains_size                     15
#define pbRxMaster_size                          90
#define pbSmallMessageGroup_size                 90
#define pbTxMaster_size                          409

oliverlee commented 6 years ago

I think we want to split pbModelCanonical into multiple messages as it is larger than the others, even though it is unlikely we want to set the matrices individually.

mickvangelderen commented 6 years ago

multiple messages as it is larger than the others, even though it is unlikely we want to set the matrices individually

It is desirable from the implementation point of view, but we can only do it iff

Configuration commands are defined for each set of configurable variables that must be changed all at once. This makes it easier to ensure sensible values are used in every simulation iteration. If values can be changed independently they should be changeable through different commands.

oliverlee / phobos