Simulink Diagram Details - Carleton-SRL/SPOT GitHub Wiki

This wiki page will provide some additional details on how the Simulink diagram Template_v4_1_0_2024b_Jetson.slx works. This page will be useful for anyone who wants to design an experiment or expand the template for their own purposes.

The template file can be sub-divided into 7 different sections.

• Guidance and Control: This section contains the experiment logic sub-system, the guidance sub-system, and the control sub-system.

• Required Memory Store Blocks: This section contains memory store blocks that are required for the software to function, thus these should never be changed.

• External Hardware Interfaces: This contains any code needed to interface with external hardware like the robotic arm, the motion tracking cameras, the thrusters, and the inertial measurement units.

• Plant Dynamics: This section contains the models used to simulate the platforms.

• Log Data: This section contains the data logging bus. If you want to add extra parameters to log, this is where you would add them.

• Resources: This section is reserved for any blocks that don't quite fit anywhere else.

• User Defined Memory Store Blocks: Lastly, this is for any user created memory store blocks.

The Guidance and Control sections are where most users will make changes.

The Experiment Logic controls which guidance and control laws are active during any given phase. When setting up a new project, start by going into each phase and defining the following three things:

1. For each active platform, define the desired x, y, and attitude endpoints. These will be passed to the path planner which then determines a path to those points.

2. Under the Control Blocks section, select which path planner you are using (custom or none). Selecting none means that your desired points are passed directly to your controller. In such a case, a simple controller (PD) - for example - would make a bee-line for the point.

3. Under the Control Blocks section, select which control law you are using (PD, custom, or disabled). Selected disabled will completely turn off the thrusters. By default, custom is an LQR.

Once you have defined the desired points for each phase of the experiment, you can move on to the Guidance subsystem to define any Path Planners. Generally, users should only change the path planners in the custom if-action subsytem - but there is nothing stopping users from adding 100s of path planners to switch between.

The path planner then sends the path for x, y, and attitude to the Control subsystem. This subsystem works the exact same as the Path Planner - users can edit the custom control law, or simply add any number of additional controllers to jump between.

Inside the Experiment Logic sub-system is a large If-Else-Action loop; each If-Action subsystem is activated based on the phase durations that the user set up in the GUI. So, Phase #0 starts at t = 0 s and stops at t = Phase0_End. There are 6 phases that a user can modify in this section of the code, as shown:

The phases are:

• Phase #0: This phase is used to wait for data from the ground truth system.
• Phase #1: This short phase is used to turn on the flotation.
• Phase #2: During this phase, the platforms will all move to their initial locations using LQR controllers.
• Phase #3: This is the main experiment phases. User cannot directly edit the duration of this phase, and are instead required to edit the 4 sub-phases. Users can add sub-phases if desired, but will have to circumvent the use of the GUI to do so - this is generally reserved for more advanced users.
• Phase #4: During this phase, the platforms will all move to their home locations using LQR controllers.
• Phase #5: During this phase, the platforms will hold their home positions.

When you open a phase If-Action subsystem, regardless of phase, you will be presented with the following:

Here we have the experiment logic for each of the platforms, as well as a section for controlling the active control law and path planner, and a section for debugging. Each of these panels contains a sub-system, Desired Position, which feeds into the corresponding memory store block. Users should change the contents of these sub-systems to define the desired final goal position for the current phase. This is the goal position that will be fed directly to the path planner, which in turn will either (1) calculate the next waypoint in the path to this final position or (2) will simply pass the final goal position directly to the active control law.

To control which path planner and control law is active for the given phase, we can use the Control Blocks section. Each platform has a memory store block called Control_Law_Enabler and Path_Planner_Selection. Users must pass a constant value to these blocks, and this will set the active planner and controller. By default, we send 2 for the control law--which is the LQR controller--and 0 to all the path planners--which disables the planners.

The debugging section allows users to send messages that can be received during an experiment in the GUI using the Open Log button. Messages must always be preceded with the tag identifying which platform is sending the message.

Generally speaking users will only need to change the contents of the Phase #3: Experiment sub-system, which itself contains 4 sub-systems. The contents of these sub-sub-systems is the same as previously described. The reason we have sub-phases is typically to ensure that your true experiment starts at rest. We typically recommend the following:

• Sub-Phase #1: Get to desired starting position for the experiment (assuming it is different from the initial position).
• Sub-Phase #2: Turn off the pucks so that the platform is completely at rest.
• Sub-Phase #3: Turn on the pucks to allow flotation.
• Sub-Phase #4: Perform the desired experiment.

Users do not need to follow this structure; in fact, simply setting the duration for these phases to 0 in the GUI will allow you to completely skip them. Inside the sub-phase If-Action blocks, users will see identical logic to what was found in the other phases. In the template file - as of writing this entry - the RED platform is following a circular trajectory around the center of the table while rotating:

Moving back to the root of the diagram, we can enter the 2 - Guidance sub-system, which contains the path planners for the diagram:

Users can add new path planners by either changing the Custom PPL sub-system, or by adding a new else-if to the system. Note that users must ensure they set the ID of the path planner correctly in the experiment logic. Navigating back up to the root of the diagram, we can now enter the 3 - Control sub-system:

The various Control Laws that will be used in any experiment are defined in this subsystem. By Default, a Proportional-Derivative controller is assigned to Control Law #1, and a Linear-Quadratic-Regulator is assigned to Control Law #2.

To add new control laws, the recommended approach is to create a new If Action Subsystem and edit the If block so that it has a new Else-If statement for u1 == 3, u1 ==4, etc. (up to however many controllers you are adding.

Then, edit the merge so that it accepts the new number of controllers. Place your new controller into the new If Action subsystem.

To use the Control Law, in the Experiment Phases change the input to the relevant Control Law Enabler to match the controller you want to use - in this example, that would be #3.

The RED control law section on this page has been configured to have one extra If Action subsystem to demonstrate how this should look when it is properly set up.

Note, any controller number that is not assigned (in this case, any number other then 1, 2, and 3) will disable the thrusters due to the Else statement.

In this section you will also see the control law for the robotic manipulator.

This section of the code is only active if the user has opted to include the manipulator in the simulations (via the GUI). If the ARM is being simulated, then the following code is also executed:

This particular logic does one of two things: if this is an experiment, then the desired joint angles are passed through directly to the actuator position controller; if this is a simulation, then the desired joint angles are passed to a PD position controller. At the moment (2025-09-07), this PD controller is NOT equivalent to the one in the Dynamixel actuators, so don't expect identical behavior. However, it should provide a good idea of what to expect. As a general rule the arm in experiment will have little to no overshoot when commanded using the position or speed controller. It is also important to note that by default the template only includes the behavior for the position controller. It is up to the user to change the controller to meet their needs. Just remember to read up on the manipulator control block so that you send the correct commands to the actuators.

There are two sub-sections that have memory store blocks: one contains required blocks, and the other contains user blocks. Simulink's Memory Store Blocks are crucial components when dealing with complex control systems and models that require the retention of information when data is being passed through If-Action blocks. This is because If-Action blocks execute conditionally depending on certain conditions, causing non-uniform execution order.

If data flows from an If-Action block to other blocks that execute at a different rate, it can lead to inconsistencies or data inconsistencies in your model. Memory Store Blocks help to alleviate this problem by maintaining the data from previous time steps in the simulation. This feature ensures that data is reliably stored and transferred regardless of the execution order or timing of various parts of your model. This stability is critical in developing robust, accurate simulations, particularly when working with intricate system dynamics or control strategies.

Right now the User Memory Store section is empty, but there are many required blocks already present. These include things like commanded forces and torques, spacecraft positions, experiment time, and so on.

When adding a new memory store block, you must have a Data Store Memory Simulink block at the top level of your diagram. Then when you write data to the block you add a Data Store Write block and pass data to the block. If you want to read the data, you add a Data Store Read block. You can write arrays to memory store blocks by changing the Initial value parameter under the Signal Attributes. The SPOT software is now designed to handle arrays.

Let's now move on to the data logging section of the template.

Navigating into the Data Logger Subsystem, users will see the following (note the screenshot below is trimmed):

This subsystem uses the demux blocks to split memory store blocks into individual signals, then combines them all using a data bus. The contents of this bus are then logged as either:

• out.dataPacket: This array only exists in simulation. After a simulation is performed, this array is saved to the workspace. An additional function built into the GUI also converts this packet into a structure to more easily identify the variables.

• Data Logger: This subsystem is a custom device driver (i.e. a C++ function) which writes the data to a text file while an experiment is performed. The data is stored as a comma-delimited data set. When the user clicks on the Save Experiment Data in the GUI, the data is copied from the target platforms onto the ground station, where it is imported and then converted to a structure (identical to what is done to the simulation data). The CustomDataLogger function is as follows:

    #include <iostream>
    #include <iomanip>
    #include <fstream>
    #include <ctime>
    
    #include "custom_data_logger.h"
    
    using namespace std;
    
    string newName = ""; 
    
    void createFile() {
        
        string name = "ExpLog";
        int i = 1;
        newName = name;
        while(true){
        
            ifstream file(newName);
            if(file.good()){
             
                newName = name + to_string(i);
                i++;
                
            } else {
            
                break;
                
            }
                   
            
        }
        
        ofstream file(newName);
        file.close();
       
    }
    
    // This function opens a text file and appends data into it.
    // The function will accept an array of unknown size, and will
    // loop through the array and append the data to the file.
    
    void appendDataToFile(double exp_data[], int num_params)
    {
        
        ofstream myfile;
        myfile.open (newName, ios::app);
        
        for (int i = 0; i < num_params-1; i++) {
            myfile << exp_data[i] << ","; 
        }
        
        myfile << exp_data[num_params-1];
        myfile << endl;
        
        myfile.close();
    }
  

The function is fairly self-descriptive, but in brief there are two functions: the createFile function takes no inputs and creates a text file with the name ExpLog*, where the * is replaced with an integer. This number is set based on how many data files exist on the platform computer. This way, no data is every accidentally overwritten. The appendDataToFile takes in an array and the number of parameters, and then writes the data to the text file that was opened using the createFile function.

Adding new data is as simple as expanding the bus, connecting a memory store block, and editing the name of the connecting wire. Note that you do not need to split of arrays, and the software will handle it properly. Splitting up the arrays has the benefit of letting you choose the names for each data vector, but without splitting them the software will name them according to their position in the array (Desired State (1), Desired State (2), etc...).

The Plant Dynamics section of the diagram contains the dynamics models for the platforms. As of SPOT 4.1.0-Release.1 the dynamics do not include Hill's equations - this may change for future versions. Users will note that this section of code is only executed if the isSim variable is equal to 1 - thus the code is not run when a experiment is being performed. This variable is set automatically by the GUI. Clicking into the Simulate Plant Dynamics subsystem, users will be presented with three additional subsystems:

• RED and ARM Dynamics: Contains the dynamics for both the RED platform alone as well as the dynamics for the case where the manipulator is attached.
• BLACK Dynamics: Contains the dynamics for the BLACK platform.
• BLUE Dynamics: Contains the dynamics for the BLUE platform.

The dynamics for all three platforms are the same for case where there is no manipulator attached to RED. Let's click on the RED and ARM dynamics subsystem as an example. Inside this subsystem is another If-Block, which takes the platformSelection as an input. This parameter is determined in the GUI when the user selects different combinations of platforms. This will be discussed in the Advanced GUI Details section of the Wiki. For now, simply know that if the platformSelection is 4, 5, 10, or 11, then the coupled RED+ARM dynamics are used; otherwise, the RED dynamics are used.

For now, let's investigate the RED Only If-Action block. Inside this block is another subsystem called RED Dynamics - click on that one as well. At this point users will see the following:

From left to right, we have the saturated forces (more on them later) going into the Red Dynamics Model subsystem. This function contains the following code:

function x_ddot = RED_dynamics(control_inputs, mRED, IRED)

    %#codegen
    % Fully define the size of x_ddot() for code generation:
    x_ddot     = zeros(3,1);
    
    % Extract the servicer model parameters from the model_param vector defined
    % in initialize.m:
    
    % Extract the control inputs from the control_input vector:
    Fx        = control_inputs(1);              % [N]
    Fy        = control_inputs(2);              % [N]
    Tz        = control_inputs(3);              % [N.m]
    
    % Calculate the state acceleration vector:
    x_ddot(1) = Fx/mRED;                      % [m/s^2]
    x_ddot(2) = Fy/mRED;                      % [m/s^2]
    x_ddot(3) = Tz/IRED;                      % [rad/s^2]

end

This function is taking in the control inputs, the mass of RED, and the inertia of RED, and is calculating the accelerations of for RED using the control inputs and Newton's Second Law (force = mass * acceleration). The accelerations are then integrated twice to get the velocity and position of the RED platform. These are then stored into their corresponding Memory Write blocks.

The dynamics for BLACK and BLUE are identical, with the only difference being their mass properties. So, let's go back up a level and take a look at how data is being stored and how noise can be added to the system. The output states from the dynamics are split into two: (1) Measured States, and (2) True States. Measured states are intended to represent how the data appears. This means measured states have noise and are downsampled to match the rate of the data coming in (which is at half the rate of the Simulink diagram). Let's enter the Data @ Measurement Rate block:

Effectively, this is just taking in the simulation states and holding them constant to match the dataRate constant. Note that this custom implementation was required due to limitations with the Simulink compiler and any built in sample and hold functions. Let's go back up to the RED and ARM Dynamics subsystem and investigate the coupled RED+ARM If-Action block. Inside this subsystem, users will see the following:

Similar to the RED dynamics, there are forces and torques being sent into a MATLAB functions simply called Dynamics Model. Inside the dynamics model is the following code:

function xdot  = DynamicsModel(x, InertiaS, CoriolisS, u,...
    Gamma1_sh, Gamma2_sh, Gamma3_sh, Gamma5_sh, Gamma4_sh, Gamma6_sh,...
    Gamma1_el, Gamma2_el, Gamma3_el, Gamma5_el, Gamma4_el, Gamma6_el,...
    Gamma1_wr, Gamma2_wr, Gamma3_wr, Gamma5_wr, Gamma4_wr, Gamma6_wr)


    fqdot1 = Gamma1_sh*(tanh(Gamma2_sh*x(10)) - tanh(Gamma3_sh*x(10))) + Gamma4_sh*tanh(Gamma5_sh*x(10)) + Gamma6_sh*x(10);

    fqdot2 = Gamma1_el*(tanh(Gamma2_el*x(11)) - tanh(Gamma3_el*x(11))) + Gamma4_el*tanh(Gamma5_el*x(11)) + Gamma6_el*x(11);

    fqdot3 = Gamma1_wr*(tanh(Gamma2_wr*x(12)) - tanh(Gamma3_wr*x(12))) + Gamma4_wr*tanh(Gamma5_wr*x(12)) + Gamma6_wr*x(12);
    
    q_ddot = pinv(InertiaS)*(u - CoriolisS*x(7:12) - [0;0;0;fqdot1;fqdot2;fqdot3]);
    
    xdot   = [ x(7:12)
               q_ddot   ];
           
end

This function does two things: first, it is calculating the friction based on this model. Then, it calculates the platform and manipulator joint accelerations, which are then output and integrated to the get the velocity and position states. Users will notice that this model takes in two matrices, InertiaS and CoriolisS. These are highly complex functions which will not be covered here. For details on these equations and where they come from, please refer to this paper. The Gamma parameters were experimentally determined. Similar to the normal models, the manipulator states are also split into true and measured states. Additionally, simulation termination conditions have been added in the Exceeded Table Limits sub-system so that when the simulation "explodes", the simulation stops.

The external hardware interface section of the diagram contains (6) subsystems:

The function of each subsystem is as follows:

• Float Code: This contains the code to float the platform, specifically a GPIO write block set to write to pin 428.
• Robotic Arm Code: This contains the code used to interface with the manipulator and send it commands.
• Manipulator Encoder Data: This contains the code used to interface with the manipulator and read the encoder values.
• PhaseSpace Camera Code: This contains the code used to receive the position data from the PhaseSpace cameras.
• Thruster Control Control: This contains the code used to command the thrusters.
• Gyro./Accel. Code: This contains the code used to log the IMU data.

Navigating into the Robotic Arm Code, users will find another If-Else block, which only activates if the user is both NOT running a simulation, and if the platform is RED (identified by the WhoAmI block). If we navigate into the Change ARM Behavior subsystem, users will find the device driver used to control the robotic manipulator. The code for this block is covered in this section of the Wiki and won't be repeated here.

Going back to the top level of the diagram, we can now navigate into the Manipulator Encoder Data subsystem. Here, users will find the same logic as in the Robotic Arm Code, which prevents this code from running in simulation and on any other platform besides RED. Clicking into the Change ARM Behavior subsystem, users will find the ReadArm_Position_Rates device driver:

https://github.com/Carleton-SRCL/SPOT/wiki/Dynamixel-Actuators-Code-Overview

This device driver reads the encoder data for the three Dynamixel actuators and returns both the angles and angular velocities. These are then stored into their corresponding Memory Write blocks. Users should also note that this device driver has 3 inputs; these inputs correspond to the joint angles from the previous time-step. The C++ function inside the driver uses this data to ensure that when no new encoder data is available, the previous state is held constant rather then returning 0. These encoders, as implemented in SPOT 4.1.0-Release.1, are slow to read data and typically return the joint angles at a rate of < 5 Hz.

Back at the top level of the diagram, we can now navigate into the PhaseSpace Camera Code. In this subsystem, users will find an If-Else statement. This statement checks if the diagram is running as a simulation. When it is a simulation, a continuous clock is used as the universal time. No states are set in this case, as they are created by the Plant subsystem.

In the case of an experiment, the Use Hardware to Obtain States If-Action block is activated.

Within this block, there are several sub-systems and a lot is happening, so let's break it down. Starting with the Receive and Process Phasespace Data:

Within this system we have a UDP receiver listening for data from the motion capture system. This data is ASSUMED to be a [19, 1] and of type double; in other words, this block will not work if the size or data type is changed. This data is reshaped before being passed into a demux block. This block splits out the time from the rest of the data packet as we currently do not use the Phasespace time data. Instead, we have another subsystem called Check for Synchronization which looks like:

All this system does is receive clock signals from three different UDP addresses. These are passed into a CheckForSync function, which checks what platforms should be active and compares that which which platforms are currently sending a clock signal. Once all platforms that should be active are sending out a clock signal, the StartClock signal is sent to the Triggered Subsystem, which then uses the time stamp from the slowest clock as the basis for all platforms. This way, experiments only start once the last active platform starts sending data.

If we go back up to investigate the Store subsystems, they are all effectively the same. So, let's look at Store RED Data:

All this is doing is converting the measurements from millimetres to meters and storing the measurements into the RED_Measured_States block. Going back to the top level of the diagram, we can open up the Thruster Control Code. This code controls the control mixer for the platforms.

The If-Statement blocks in this section of code check if a simulation is running (in which case everything is executed), or if an experiment is running (in which case only the code for RED, BLACK, or BLUE is executed - depending on which platform the code is running on). Let's use RED as an example and open the Change RED Behavior subsystem. Inside, we see the following:

This subsystem contains two other subsystems. The first one, Rotate Forces to Body, contains the following logic:

This takes the forces calculated in the inertial frame and rotates them into the body frame. This is done using a rotation matrix C_bI under inside the Create Rotation Matrix MATLAB User Defined Function:

function C_bI = RotateFrame(Rz)

    C_bI = [  cos(Rz) sin(Rz)
             -sin(Rz) cos(Rz) ];

end

Going back up two levels we can see that the body forces are being passed alongside the commanded torque into the subsystem called Calculate Duty Cycle. Inside this subsystem is a fairly lengthy piece of logic. So, we will break it down in chunks, moving from left to right. Because this process ultimately relies on using a custom least-squares solution, we first need an initial guess at the required duty cycles for the thrusters; these can then be iteratively improved.

This code calls the function MakeH. This is the thrust distribution matrix; these take the desired thrust and torque, and converts them into individual thruster cycles. Here is the code for MakeH:

function H = MakeH(thruster_dist2CG_RED,F_thrusters_RED)

    Vec1 = [ -1
             -1
              0 
              0
              1
              1
              0
              0 ];
          
    Vec2 = [  0
              0
              1
              1
              0
              0
             -1
             -1 ];
         
    Vec3 = thruster_dist2CG_RED./1000;
    
    Mat1 = [Vec1, Vec2, Vec3]';
    
    Mat2 = diag((F_thrusters_RED./2));
    
    H    = Mat1*Mat2;

end

This is a [3,8] matrix. The first row represents the approximate forces being applied in the X-direction. The second row represents the same, but for the Y-direction. The last row represents the torques applied by each thruster on the platform COG. The output matrix is passed into a pseudo-inverse function. Once the pseudo-inverse has been calculated, it is multiplied by the commanded forces and torque in the body-fixed frame. This calculation results in duty cycle. This is then passed, alongside the desired forces, to the optimize_duty_cycle_with_decay function:

function [H_final, duty_cycles] = optimize_duty_cycle_with_decay( ...
    u_desired, x0, thruster_dist2CG_RED, F_red_X_nominal, F_red_Y_nominal, max_iters, tol)
    %OPTIMIZE_DUTY_CYCLE_WITH_DECAY 
    %   Iteratively optimizes thruster duty cycles while updating thrust decay.
    %
    %   Inputs:
    %       u_desired           - Desired force/torque vector [Fx; Fy; M] (3x1)
    %       x0                  - Initial guess for duty cycles (8x1)
    %       thruster_dist2CG_RED- Distances of thrusters from CG (vector, mm)
    %       F_red_X_nominal     - Nominal X-direction thruster force (scalar, N)
    %       F_red_Y_nominal     - Nominal Y-direction thruster force (scalar, N)
    %       max_iters           - Maximum optimization iterations
    %       tol                 - Convergence tolerance (on duty cycles)
    %
    %   Outputs:
    %       H_final     - Final H-matrix including thrust decay (3x8)
    %       duty_cycles - Optimized duty cycles (8x1)
    %

    % ---------------------------
    % Initialization
    % ---------------------------
    num_thrusters      = length(x0); %#ok<NASGU> % reserved if needed later
    duty_cycles        = x0;                     % current solution
    thrust_decay_factor = 1;                     % initial decay
    prev_duty_cycles   = duty_cycles + 2*tol;    % force first iteration

    % ---------------------------
    % Iterative optimization
    % ---------------------------
    for iter = 1:max_iters
        % Build system matrix H with current decay factor
        H = MakeHWithDecay(thruster_dist2CG_RED, F_red_X_nominal, ...
                           thrust_decay_factor, F_red_Y_nominal);

        % Solve constrained least squares (box constraints: 0 ≤ x ≤ 1)
        duty_cycles = custom_constrained_least_squares(H, u_desired, duty_cycles);

        % Update decay factor based on the current duty cycles
        [thrust_decay_factor, ~] = check_thrust_decay(duty_cycles);

        % Check convergence (∞-norm difference)
        if norm(duty_cycles - prev_duty_cycles, Inf) < tol
            H_final = MakeHWithDecayRecalculateForce(thruster_dist2CG_RED, ...
                        F_red_X_nominal, thrust_decay_factor, F_red_Y_nominal);
            return;
        end

        % Update reference for next iteration
        prev_duty_cycles = duty_cycles;
    end

    % If not converged within max_iters, return final H with last decay
    H_final = MakeHWithDecayRecalculateForce(thruster_dist2CG_RED, ...
                F_red_X_nominal, thrust_decay_factor, F_red_Y_nominal);
end


% ========================================================================
% Custom Constrained Least Squares Solver
% ========================================================================
function x_opt = custom_constrained_least_squares(H, b, x0)
    %CUSTOM_CONSTRAINED_LEAST_SQUARES
    %   Solves min_x ||H*x - b||^2 subject to 0 <= x <= 1
    %   using projected gradient descent with backtracking line search.
    %
    %   Inputs:
    %       H   - System matrix (3x8)
    %       b   - Desired vector (3x1)
    %       x0  - Initial guess (8x1)
    %
    %   Output:
    %       x_opt - Optimal duty cycle vector (8x1)

    % Parameters
    max_iterations = 100;
    tolerance      = 1e-6;
    alpha          = 0.01;  % initial step size
    beta           = 0.5;   % backtracking factor

    % Initialization
    x      = x0;
    x_prev = x0 + 2*tolerance;

    % Precompute matrices
    HtH = H' * H;
    Htb = H' * b;

    for iter = 1:max_iterations
        % Gradient of ||H*x - b||^2
        gradient = 2 * (HtH*x - Htb);

        % Store objective for comparison
        current_obj = norm(H*x - b)^2;

        % Backtracking line search
        step_size = alpha;
        for ls_iter = 1:20
            x_new = x - step_size * gradient;
            x_new = max(0, min(1, x_new)); % project into [0,1]

            new_obj = norm(H*x_new - b)^2;
            if new_obj < current_obj
                break; % accept step
            else
                step_size = step_size * beta;
            end
        end

        % Update
        x = x_new;

        % Convergence check
        if norm(x - x_prev) < tolerance
            break;
        end

        % Adaptive step size tuning
        if iter > 1 && new_obj < current_obj
            alpha = min(alpha * 1.1, 0.1); % speed up if progress
        else
            alpha = alpha * 0.9;           % slow down otherwise
        end

        % Store for next iteration
        x_prev = x;
    end

    x_opt = x;
end


% ========================================================================
% Construct H-matrix (with decay factor)
% ========================================================================
function H = MakeHWithDecay(thruster_dist2CG, F_X_nominal, thrust_decay_factor, F_Y_nominal)
    %MAKEHWITHDECAY
    %   Builds the H matrix that maps thruster duty cycles to [Fx; Fy; M],
    %   applying thrust decay.
    %
    %   Outputs H as (3x8)

    % Convert thruster arm distances to meters
    adjusted_dist = thruster_dist2CG / 1000;

    % Thruster directions (X, Y, and moment arm)
    Vec1 = [-1; -1;  0;  0;  1;  1;  0;  0]; % X
    Vec2 = [ 0;  0;  1;  1;  0;  0; -1; -1]; % Y
    Vec3 = adjusted_dist;                    % moment (Z)

    Mat1 = [Vec1, Vec2, Vec3]';

    % Thruster forces (scaled by decay)
    F_thrusters = [F_X_nominal; F_X_nominal; ...
                   F_Y_nominal; F_Y_nominal; ...
                   F_X_nominal; F_X_nominal; ...
                   F_Y_nominal; F_Y_nominal] * thrust_decay_factor;

    % Build diagonal scaling
    Mat2 = diag(F_thrusters / 2);

    % Final mapping
    H = Mat1 * Mat2;
end


% ========================================================================
% Construct H-matrix (recalculate full force with decay)
% ========================================================================
function H = MakeHWithDecayRecalculateForce(thruster_dist2CG, F_X_nominal, thrust_decay_factor, F_Y_nominal)
    %MAKEHWITHDECAYRECALCULATEFORCE
    %   Same as MakeHWithDecay but applies full nominal force scaling.

    adjusted_dist = thruster_dist2CG / 1000;

    Vec1 = [-1; -1;  0;  0;  1;  1;  0;  0];
    Vec2 = [ 0;  0;  1;  1;  0;  0; -1; -1];
    Vec3 = adjusted_dist;

    Mat1 = [Vec1, Vec2, Vec3]';

    F_thrusters = [F_X_nominal; F_X_nominal; ...
                   F_Y_nominal; F_Y_nominal; ...
                   F_X_nominal; F_X_nominal; ...
                   F_Y_nominal; F_Y_nominal] * thrust_decay_factor;

    Mat2 = diag(F_thrusters);

    H = Mat1 * Mat2;
end


% ========================================================================
% Thrust Decay Update Rule
% ========================================================================
function [thrust_decay_factor, count] = check_thrust_decay(PWMs)
    %CHECK_THRUST_DECAY
    %   Updates thrust decay factor based on average duty cycle.
    %
    %   Inputs:
    %       PWMs - Duty cycle vector (8x1)
    %
    %   Outputs:
    %       thrust_decay_factor - Adjusted decay factor (scalar)
    %       count               - Number of active thrusters

    % Normalize if needed
    if max(PWMs) > 1
        PWMs = PWMs / max(PWMs);
    end
    
    % Active thrusters
    count = sum(PWMs > 0);
    
    % Mean duty cycle (only active thrusters)
    avg_duty = sum(max(PWMs, 0)) / max(count, 1);
    
    % Update rule (piecewise)
    if avg_duty < 0.3 || count == 0
        thrust_decay_factor = 1;
    else
        thrust_decay_factor = max(0.6 - 2*avg_duty + 1, 0.5);
    end
end

Briefly, the optimize_duty_cycle_with_decay function refines the initial duty cycle estimates through an iterative optimization that also includes thrust degradation under continuous operation. At each iteration, the algorithm reconstructs the H matrix with an updated thrust decay factor, solves a constrained least-squares problem to determine duty cycles bounded between 0 and 1, and recalculates the decay factor based on the average duty cycle of active thrusters. The decay factor follows a piecewise function that maintains nominal thrust (factor = 1.0) for average duty cycles below 30%, then progressively reduces thrust output to a minimum of 50% for higher duty cycles. The optimization employs projected gradient descent with backtracking line search to solve the constrained least-squares problem; once the cost function (which is the match between the desired forces and torque and that which can be achieved through the duty cycles) is below the target tolerance, the optimization stops.

Going back up to the Thruster Control Code, the output of the If-Action subsystems is demuxed into 8 individual signals for all three platforms. These are then summed up and passed to a UDP block. The summation of these signals works because in an experiment, only one of the If-Action subsystems is running on a given platform. The other two are not running, and thus output a vector of zeros. This way, we only need one UDP block for the entire diagram. For details on the software PWM code, refer to the relevant Wiki page.

Lastly, under the Gyro./Accel. Code we find the following:

This code just uses the BMI160 IMU built-in function from Simulink to read in the data from the sensors. There is nothing overly complex here. Th data from the sensors is then parsed into the correct data storage location based on which platform the code is running on.

Navigating back to the root of the template, we can now dig into the final section of the diagram: Resources.

This section of the template contains 7 sub-systems:

• Platform Identification: This is a Device Driver which identifies which of the three platforms the code is currently running on.
• Check Connection: This block is not currently being used and will be removed in a future release.
• Simulation Status: This contains a single Memory Store block for the isSim parameter.
• Send Data to Orin: This contains a UDP Send block which sends the positions and velocities of all three platforms to the NVIDIA Jetson Orin.
• Debugger: This takes in the log messages and parses them before sending the messages to the GUI over UDP.
• Receive Data from Orin: This listens for data coming from the vision computer, and by default excepts 4 data points of type single.
• Time Sync.: This effectively replaces what Check Connection used to handle, and simply sends out a clock signal that can be intercepted by the different platforms.

The other sub-systems are self-explanatory, so let's focus on the Platform Identification. Navigating into this subsystem, users will see the an If-Action block that only runs in experiments. Navigating into this action subsystem, users will find a single device driver labelled IdentifyPlatform. The source code for this driver can be located here, but for completeness here is the code:

#include <iostream>
#include <unistd.h>
#include <cstring>

#include "resource_headers.h"

int getComputerIdentifier(const char* hostname) {
    if (std::strcmp(hostname, "spot-red") == 0) {
        return 1;
    } else if (std::strcmp(hostname, "spot-black") == 0) {
        return 2;
    } else if (std::strcmp(hostname, "spot-blue") == 0) {
        return 3;
    } else if (std::strcmp(hostname, "spot-spare") == 0) {
        return 4;
    } else if (std::strcmp(hostname, "spot-vision") == 0) {
        return 5;
    } else {
        return 0;  // No match found
    }
}

int WhoAmI() {
    char hostname[256];
    if (gethostname(hostname, sizeof(hostname)) == 0) {
        int identifier = getComputerIdentifier(hostname);
        return identifier;
    } else {
        std::cerr << "Failed to get the hostname." << std::endl;
        return 1;
    }

    return 0;
}

In essence, this code retrieves the hostname of the computer that the code is running on. It compares the retrieved hostname with these specific strings: "spot-red", "spot-black", "spot-blue", "spot-spare", and "spot-vision" and returns corresponding integer identifiers.

First, the getComputerIdentifier() function takes the retrieved hostname as an input and compares it with the specified strings using std::strcmp(). If a match is found, the function returns the corresponding identifier (1 for "spot-red", 2 for "spot-black", and 3 for "spot-blue", for example). If there is no match, the function returns 0 as the default value.

In the whoAmI() function, the code retrieves the hostname using gethostname() and calls getComputerIdentifier() to obtain the identifier based on the matched hostname. The identifier is returned as an integer and is used in identifying the active platform.

At this point, you should be well prepared to start editing your diagram. I would like to take this opportunity to indicate that the template diagram is just that: a template. You can make small changes, or you can make sweeping changes that fundamentally change the approaches presented here. Adam Vigneron has an excellent example of this here: https://github.com/adamvigneron/AdamVigneron-SPOT

His approach completely does away with using Simulink logic and instead relies on class wrappers to manage the simulation and experiment logic. Although this can be less obvious for new users, his approach significantly improves version control and is much more representative of how this kind of software looks in industry. I highly recommend advanced users take a look, perhaps you'll be inspired!

Click here to go HOME