%% Field-Oriented Control of Permanent Magnet Synchronous Machine
% This example shows the basic workflow and key APIs for generating C 
% code from a motor control algorithm, and for verifying its compiled 
% behavior and execution time. You will use Processor-in-the-loop (PIL) 
% simulation to gain confidence that the C code will perform as expected
% when you integrate it with embedded software that interfaces with the 
% motor hardware. Although this workflow uses a motor control 
% application for a specific processor, you can apply this workflow 
% to virtually any application or processor.
%
% <<../rtwdemo_pmsmfoc_workflow.png>>
%
% We use a Field-Oriented Control algorithm for a Permanent Magnet Synchronous 
% Machine to show this workflow. This control technique is common in motor 
% drive systems for hybrid electric vehicles, manufacturing machinery, and
% industrial automation. 
%
%% Overview
% In this example, generate and verify C code from a control algorithm 
% model, which you can integrate with additional embedded software required 
% to interface to motor hardware.
%
% You will use a simulation environment to model and verify the behavior of
% a closed-loop motor control system. Once the control system behavior is
% within specification, you will generate C code from the controller model.
% After inspecting the code, you will assess its functional behavior and
% execution time using processor-in-the-loop (PIL) testing. 
%
% <<../rtwdemo_pmsmfoc_pil.png>>
%
% To facilitate PIL testing, you will select test signals to exercise the
% controller model and establish reference outputs. You will review an
% example PIL implementation for a Texas Instruments(TM) F28335 processor that
% communicates with Simulink(R) on the host computer over a serial connection.
% You can use this example as a starting point to create a PIL
% implementation for your own processor. You will run the controller model
% in PIL mode to measure execution time and verify execution behavior of
% the code running on the embedded processor against the simulation
% reference outputs.
%
% In the final implementation on the embedded processor, you would
% integrate the generated controller C code with additional embedded
% software, such as peripherals and interrupts, required to interface with
% the motor hardware.    
%
% *Notes*
%
% * Simscape(TM) Electrical is required for system simulation in the 
% section "Verify Behavior through System Simulation." It is not required for other tasks.
% * The Texas Instruments(TM) F28335 PIL implementation is a reference
% approach that you can apply to virtually any processor. However, if you
% wish to use this implementation directly, you will need additional
% support files, compilers, and tools from Texas Instruments(TM). You can
% find more information about those in "Create PIL Implementation and
% Register with Simulink(R)" section of this example. This reference PIL
% implementation does not require the Texas Instrument C2000(TM) Embedded
% Target feature of Embedded Coder(R), but C2000(TM) users are encouraged
% to install Texas Instruments C2000 Support Package using
% <matlab:supportPackageInstaller Add-On Explorer>.

%   Copyright 2010-2016 The MathWorks, Inc.

%% Verify Behavior through System Simulation
% In this section, you will verify the controller in a closed loop system simulation. 
%
% The system model test bench consists of test inputs, the embedded
% processor, the power electronics and motor hardware, and visualizations.
% You can use the system model to exercise the controller and
% explore its expected behavior. You can use the following commands to
% execute the model and plot logged signals.
open_system('rtwdemo_pmsmfoc_system')
out_system = sim('rtwdemo_pmsmfoc_system')
rtwdemo_pmsmfoc_plotsignals(out_system.logsout)

%%
% The plots show that the motor is stationary until the |motor_on| signal
% is true. The motor then spins in an open loop until a known position is
% detected, which is indicated by the encoder index pulse. The controller
% then transitions to closed loop operation and the motor reaches a steady
% state speed.

%% Explore Model Architecture
%
% In this section, you will explore the model architecture including how
% data is specified, how the controller is partitioned from the test bench,
% and how controller is scheduled. This architecture facilitates system
% simulation, algorithmic code generation, and PIL testing.
%
% A data definition file creates the MATLAB(R) data required for simulation
% and code generation. This data file is automatically run within the
% |PreLoadFcn| callback of the system test bench model.
edit('rtwdemo_pmsmfoc_data.m')

%%
% Within the system test bench model, the embedded processor is modeled as a combination of the peripherals and the controller software. 
open_system('rtwdemo_pmsmfoc_system/Embedded Processor')

%%
% The controller software is specified in a separate model. Within this model, the Mode_Scheduler subsystem uses Stateflow(R) to schedule different modes of operation of the Motor_Control algorithm.
open_system('rtwdemo_pmsmfoc')

%%
% Within the Motor_Control subsystem, sensor signals are converted to engineering units and passed to the core controller algorithm. The controller algorithm calculates voltages. The voltages are then converted to a driver signal.
open_system('rtwdemo_pmsmfoc/Motor_Control')

%%
% The primary control law is a field-oriented controller. The controller has a low rate outer loop that controls the speed, and a higher rate inner loop that controls the current.
open_system('rtwdemo_pmsmfoc/Motor_Control/Field_Oriented_Controller')

%%
% The Velocity Controller outer loop is executed as a multiple of the Current Control loop time. You can view the MATLAB(R) variables, which specify these sample times:
fprintf('High rate sample time = %f seconds\n', ctrlConst.TsHi)
fprintf('Low rate sample time  = %f seconds\n', ctrlConst.TsLo)

%%
% Notice that the highest rate in the controller algorithm is 25 kHz.
fprintf('High rate frequency = %5.0f Hz\n', 1/ctrlConst.TsHi)

%% Generate Controller C Code for Integration into Embedded Application
% In this section, you will generate and visually inspect the C code function for the controller. 
%
% To ease integration, the controller model is configured in single-tasking mode so that the generated code can be invoked using one function call. This function handles the lower and higher rates. 
% The generated controller function must be executed at the high rate sample time.
%
% The function prototype is specified in the model configuration parameters and the input and output ports are passed as arguments. You can view the function specification for the controller algorithm.
mdlFcn = RTW.getFunctionSpecification('rtwdemo_pmsmfoc');
disp(mdlFcn.getPreview('init'))
disp(mdlFcn.getPreview('step'))

%%
% Through use of a global structure in the generated code, you can access the field-oriented controller proportional and integral gains. This global structure is specified in the data definition file.
disp(ctrlParams.Value)
disp(ctrlParams.CoderInfo)

%%
% You generate C code from the model as follows.
rtwbuild('rtwdemo_pmsmfoc')

%%
% Use the generated report to inspect the generated C code file and verify
% that the correct step and initialization functions were generated. Also
% verify that the parameter structure was created as a global variable.

%% Establish Reference Behavior for Controller Model
% In this section, you will establish test inputs and a reference output to help verify behavior and profile execution time during PIL testing. You will make a local copy of the controller model then load a set of test input signals that exercise the different modes within the controller. You will then configure the controller model to attach these logged signals to input ports, execute the controller model, and log the output port signal to the workspace. 
%
% Configuration parameters of the controller model used to establish the
% reference behavior and test environment will change as described below.
% Blocks and parameters used to specify the controller model's design and
% generate production code will not change. However, to avoid modifying any
% part of the installed controller model, save the model and change its
% name to |rtwdemo_pmsmfoc_local.slx|.
save_system('rtwdemo_pmsmfoc','rtwdemo_pmsmfoc_local.slx')
close_system('rtwdemo_pmsmfoc_system',0);
close_system('rtwdemo_pmsmfoc',0);

%%
% To profile execution times, select a set of test inputs that will execute the paths of interest within the controller. One way to acquire these test inputs and reference output is to log them from the system simulation model. 
in.motor_on      = out_system.logsout.getElement('motor_on').Values;
in.command_type  = out_system.logsout.getElement('command_type').Values;
in.command_value = out_system.logsout.getElement('command_value').Values;
in.sensors       = out_system.logsout.getElement('sensors').Values;
display(in)

%%
% These signals can now be attached to the input ports and imported into the controller model such that it can execute directly and independently from the system model. An advantage of this approach is that the controller model can be tested and verified as a standalone component, facilitating reuse and integration with other system models or closed-loop test benches. 
% To elaborate, or prepare, the controller model for testing, change its Configuration Parameters to attach input signals and log signals in the MATLAB(R) workspace. 
% These changes can be made graphically in the model's Configuration Parameters dialog, or programmatically as shown below.
set_param('rtwdemo_pmsmfoc_local',...
    'LoadExternalInput', 'on',...
    'ExternalInput', 'in.motor_on, in.command_type, in.command_value, in.sensors',...
    'StopTime','0.06',...
    'ZeroInternalMemoryAtStartup','on',...
    'SimulationMode', 'normal')
save_system('rtwdemo_pmsmfoc_local.slx')

%%
% You can now execute the controller model and plot the signal associated with the PWM Compare output port. 
out = sim('rtwdemo_pmsmfoc_local')
controller_mode = out.logsout.getElement('controller_mode').Values;
pwm_compare_ref = out.logsout.getElement('pwm_compare').Values;
rtwdemo_pmsmfoc_plotpwmcompare(controller_mode, pwm_compare_ref)

%%
% The logged output will be used as the reference behavior for PIL testing. 
%
% Notice that the plot is annotated with information about the mode of the controller at each time step. This mode information will be useful when interpreting execution profiling information. 

%% Create PIL Implementation
% In this section, you will study and use an example PIL implementation. You will begin by reviewing prerequisite help documentation from Embedded Coder(R). You will then copy the example PIL implementation into your local directory and register it with Simulink(R). You will review the approach used to develop the PIL implementation and can explore the associated files to gain additional insight. If you are using a Spectrum Digital Inc. eZdsp F28335 board with Code Composer v4 and a serial connection, you will be able to configure this PIL implementation to directly work with the controller model. If you are using a different processor, you can use this PIL implementation as a starting point to create your own implementation.
% 
% The fundamentals of creating a custom PIL implementation are described in 
% <docid:ecoder_ug.bruy_tf-1 Create PIL Target Connectivity Configuration>
% . You should familiarize yourself with the basic concept of using the rtiostream API to facilitate communication between Simulink(R) (host-side) and the embedded processor (target-side) during PIL testing. Note that Embedded Coder(R) provides host-side drivers for the default TCP/IP implementation (for all platforms supported by Simulink(R)) as well as a Windows(R) only version for serial communication. Building the generated code is accomplished using a makefile as described in 
% <docid:rtw_ug.bse3c8c-1 Customize Template Makefiles>
% . To create a PIL implementation you will need to perform several tasks in the embedded environment including writing a target-side communications driver, writing a makefile to build the generated code, and automating download and execution of the built executable.
%
% <<../rtwdemo_pmsmfoc_pilapi.png>>
%
% Using the above approach, a PIL implementation was created for a Spectrum Digital Inc. eZdsp F28335 board. Below is a summary of the target connectivity API components used in this implementation.
%
% * *Host-Side Communication* - The host-side connectivity driver is configured to use serial communication.
% * *Target-Side Communication* - The target-side communication is achieved using a handwritten serial implementation of the rtiostream functions as well as timer access functions.
% * *Build Process* - A makefile based approach is used to build the executable application.
% * *Launcher* - Downloading and running the executable is accomplished using the Debug Server Scripting (DSS) utility of Code Composer Studio(TM) v4 (CCSv4).
%
% The PIL implementation was iteratively developed in three stages. Below is a description of these stages and the tasks performed in these stages. You may find it helpful to follow a similar approach when developing your own PIL implementation
%
% *Stage 1: Create serial communication application with CCSv4*
%
% * Install CCSv4 and verify that it can connect with the F28335 eZdsp board.
% * Write an embedded application that sends and receives serial data.
% * Test the serial communication between the host computer and the embedded application.
% * Identify the commands and options for the compiler, linker, and archiver to build the application using a makefile.
% * Download and run the application from a Windows(R) Command Prompt using the DSS utility.
%
% *Stage 2: Implement and test embedded serial rtiostream and launch automation with MATLAB(R)*
%
% * Extend the serial application to implement the rtiostream API functions
% for echoing data. Write the rtIOStreamOpen to perform general board
% initialization, including configuring the serial port.
% * Verify sending and receiving serial data with the embedded processor
% from MATLAB(R) using the rtiostream_wrapper function.
% * Download and run the application from MATLAB(R) using the system
% command to call the DSS utility.
%
% *Stage 3: Implement and test connectivity configuration with Simulink(R)* 
%
% * Create a connectivity configuration class to configure the host-side
% serial communication, specify which target-side code files from the
% rtiostream application should be included in the build process, specify
% how to access a timer that will be used to collect profiling data, and
% integrate calling the DSS utility to launch the embedded application.
% * Create a tool specification makefile (|target_tools.mk|) which
% specifies the commands and options for the compiler, linker, and
% archiver. This makefile is included by the template makefile
% (|target_tools.mk|).
% * Create a template makefile (|ec_target.tmf|) that includes |target_tools.mk|.
% * Identify parameters which may be installation dependent and store them
% as MATLAB(R) preferences.
% * Create a Simulink(R) customization file which specifies when the PIL
% implementation is valid
%
% The files associated with this PIL implementation are included with
% Embedded Coder(R), but are not on the MATLAB(R) path. To explore these files,
% you can copy them into a local directory. You can register this PIL
% implementation by adding this directory to the MATLAB(R) path and
% refreshing the Simulink(R) customizations.

%copyfile(fullfile(matlabroot,'toolbox','rtw','rtwdemos','examplePilF28335'),'examplePilF28335','f')
addpath(genpath(fullfile(matlabroot,'toolbox','rtw','rtwdemos','examplePilF28335')))
sl_refresh_customizations

%%
% MATLAB(R) preferences are used to specify path information and the host
% serial COM port number. If you are using this PIL implementation
% directly, you must specify these preferences as appropriate for your
% configuration.
%
% Note that the |TI_F28xxx_SysSWDir| preference points to a directory
% provided by Texas Instruments(TM) in their *C2000(TM) Experimenter Kit
% Application Software* (|sprc675.zip|). These files are not included with
% Embedded Coder(R).
%
setpref('examplePilF28335','examplePilF28335Dir', fullfile(matlabroot,'toolbox','rtw','rtwdemos','examplePilF28335'));
setpref('examplePilF28335','CCSRootDir',          'C:\Program Files\Texas Instruments\ccsv4');
setpref('examplePilF28335','TI_F28xxx_SysSWDir',  'C:\Program Files\Texas Instruments\TI_F28xxx_SysSW');
setpref('examplePilF28335','targetConfigFile',    fullfile(matlabroot,'toolbox','rtw','rtwdemos','examplePilF28335','f28335_ezdsp.ccxml'));
setpref('examplePilF28335','baudRate',            115200);
setpref('examplePilF28335','cpuClockRateMHz',     150);
setpref('examplePilF28335','boardConfigPLL',      10);
setpref('examplePilF28335','COMPort',             'COM4');

%%
% The PIL implementation is now ready to be used.

%% Prepare Controller Model for PIL Testing
% In this section you will configure the controller model to use the PIL implementation. You will review the customization file used to register the PIL implementation, set the configuration parameters of the model to use the PIL implementation, and enable logging controller outputs and execution profiling data.
%
% When you begin a simulation in PIL mode, Simulink(R) will check if any of the registered PIL implementations are valid. A customization file specifies which configuration parameters correspond to a valid PIL implementation. You can explore the customization file for this implementation by invoking the following command.
edit(fullfile(matlabroot,'toolbox','rtw','rtwdemos','examplePilF28335','sl_customization.m'));

%%
% Notice that this file specifies setting for the hardware device and the template makefile which are required to use this PIL implementation. You can modify the configuration parameters in the controller model to match these settings. These changes can be made graphically in the model's Configuration Parameters dialog, or programmatically as shown below.
set_param('rtwdemo_pmsmfoc_local',...
       'ProdHWDeviceType', 'Texas Instruments->C2000',...
       'TemplateMakefile', 'ec_target.tmf',...
       'GenCodeOnly', 'off',...
       'SimulationMode', 'processor-in-the-loop (pil)')

%% 
% You can specify collecting execution profiling information during PIL testing by logging the simulation output values as the variable |pilOut| and logging the execution profiling information as the variable |executionProfile|. These changes can be made graphically in the model's Configuration Parameters dialog, or programmatically as shown below.
set_param('rtwdemo_pmsmfoc_local',...
    'CodeExecutionProfiling', 'on',...
    'CodeExecutionProfileVariable','executionProfile',...
    'CodeProfilingSaveOptions','AllData');
save_system('rtwdemo_pmsmfoc_local.slx')

%%
% The controller model is now ready to be run in PIL mode.

%% Test Behavior and Execution Time of Generated Code
% In this section you will run the controller model in PIL mode and explore the behavioral and execution profiling results. You will verify that the behavior of the compiled controller code matches the reference simulation behavior, and then verify that the execution of the code meets timing requirements. 
%
% You can run the model and plot the PIL simulation results. When you start
% the model for the first time, Embedded Coder(R) will generate code for the
% algorithm, link the algorithm code with the serial communication
% interface code, build the embedded application, download the application
% to the board, and begin the on-target simulation. Note that during
% subsequent PIL simulations, code is only regenerated if the model
% changes. Due to the overhead associated with the serial communication
% interface, the PIL simulation may run slower than the model in normal
% mode.
%
% The MATLAB(R) commands below are intentionally commented out as they require connection to hardware and use of embedded development tools described previously. If you have the hardware connected and embedded development tools installed, uncomment and execute these lines to run the model, plot the results, and verify the behavior is numerically equivalent to the simulation running in normal mode. Otherwise, continue reviewing this section to learn about PIL execution analysis options.

% UNCOMMENT THE BELOW LINES TO RUN THE SIMULATION AND PLOT THE RESULTS
% if exist('slprj','dir'), rmdir('slprj','s'); end
% out = sim('rtwdemo_pmsmfoc_local')
% pwm_compare_pil = out.logsout.getElement('pwm_compare').Values;
% rtwdemo_pmsmfoc_plotpwmcompare_pil(controller_mode, pwm_compare_pil, executionProfile)

%%
% <<../rtwdemo_pmsmfoc_plotpil.png>>
%

%%
% The upper plot is the output of the controller, PWM Compare. Note that the outputs in PIL mode look the same as the outputs of the simulation in normal mode shown in the section "Establish Reference Behavior for Controller Model". You can subtract the outputs of normal mode simulation from the PIL mode simulation output to verify that they are numerically equivalent:

% UNCOMMENT THE BELOW LINE TO VERIFY NUMERICAL EQUIVALENCE OF THE OUTPUTS
% pilErrorWithRespectToReference = sum(abs(pwm_compare_pil.Data - pwm_compare_pil.Data))

%%
%   pilErrorWithRespectToReference =
%
%     0     0     0
     
%%
% The lower plot is the amount of time spent executing the controller model at each simulation time step. The "Stand By" state requires the least time. Small periodic spikes in execution time occur because the controller is multi-rate and single tasking. The periodic spikes correspond to the time required to run both the base rate and 5 millisecond rate code in the same task.
%
% Since the controller must be executed at 25 kHz on the embedded processor, the algorithm must complete its execution within 40 microseconds (minus additional headroom requirements by other code, which may also be executing on the final application.) The profiling results indicate that the algorithm will execute within the time allotted for this configuration of the embedded environment.
%

%%
% The generated code is now verified to provide numerically equivalent results and meets the execution timing requirements for this test case.
close_system('rtwdemo_pmsmfoc_local',0);
close_system('power_utile',0);

%% 
% The MATLAB(R) preferences used in this PIL implementation are persistent
% between MATLAB(R) sessions. If you want to remove these preferences,
% run the following commands.
rmpref('examplePilF28335');
rmexamplePilF28335hooks();

%% Conclusion
% This example showed system level simulation and algorithmic code
% generation using a Field-Oriented Control algorithm for a Permanent
% Magnet Synchronous Machine to explore functional behavior of the
% controller algorithm. It also showed a general approach for target
% integration, functional testing, and execution profiling for any embedded
% processor. Once the algorithm was behaviorally correct, code was
% generated from the controller model, tested on the target processor and
% profiled. The algorithm code is now verified and can be integrated with
% embedded software that interfaces with the motor hardware for further
% testing.

