File content as of revision 34:0522cebfe489:

//Warning: Incompatible redefinition of macro "MBED_RAM_SIZE"  in "tmp/HU5Hqj", Line: 39, Col: 10
#ifndef MBED_RAM_SIZE
#define MBED_RAM_SIZE 0x00018000
#endif

//      ===========================================================
//      ===========================================================
// LA:  Stage 01 cleared,   Il posizionatore funziona CORRETTAMENTE
//      ===========================================================
//      ===========================================================
//
//  Commit & Publish del 11 Aprile 2022, il Pozizionamento è corretto e testato.
//  La velocità imposta all'asse, tuttavia, NON E' retroazionata.
//
//  Occorrerebbe un anello chiuso sulla velocità, che si muova "almeno" alla velocità di scansione del PWM.
//  Se la velocità non è ancora raggiunta, la tensione deve venire adeguata conseguentemente agendo sul PWM.
//

#include "QEI.h"
#include "SWPos.h"

#include "Timers.h"
#include "Eeprom.h"

#define MAX_CHAR_PER_LINE   28
#define TEXT_ROW_SPACING    16
#define FONT_CHAR_WIDTH 8
#define FONT_CHAR_HEIGHT    16

#include "DisplayDriver.h"

const float cf_SCOPeriod_s = 0.00025000;    //  250us
const float cf_PWMPeriod_s = 0.01000000;    //  10ms
const float cf_MOTPeriod_s = 0.01000000;    //  10ms

#include "PID.h"
//
#define Lx 0.0001
#define Ku 0.32200//0.922000
#define Pu 0.0125
#define Kp Ku*0.6
#define Ti Pu*0.5
#define Td Pu*0.125
//
#define P Kp
#define I Kp/Ti
#define D Kp*Td
#define Mstate 6
#define SorT 5
#define Databit 0
#define TIME 0.0125

//#define RATE (cf_MOTPeriod_s * 10.0f)
#define RATE (cf_MOTPeriod_s)

//Timer Time;
//PID SpeedClosedLoop(P,I,D,&Time);

//                          Kc,  Ti,  Td,  interval
PID PID_VelocityClosedLoop (1.0, 0.0, 0.0, RATE);
//PID PID_VelocityClosedLoop (P, I, D, RATE);

//const int64_t   ci64_TargetPOS =    240;   //
//const int64_t   ci64_TargetPOS =    3096;   //

int64_t ci64_TargetPOS =    3096;   //  Used as CONST ...

// LA:  LCM_ShowTactics
//      ===============
//
void    LCM_ShowTactics(
                            int64_t i64_Pulses,
                            int32_t i32_ATVSpeed,
                            //
                            float   f_ai0000_Aux,
                            float   f_ai0001_Aux,
                            float   f_ai0002_Aux,
                            float   f_ai0003_Aux,
                            float   f_ai0004_Aux,
                            float   f_ai0005_Aux,
                            //
                            int32_t i32_Velocity,
                            int32_t i32_Acceleration,
                            int32_t i32_Jerk

                        );

// LA:  SampleAndStore
// LA:  SampleTimer ==
//      ==============
//
static  void    SampleAndStore  (void);
Ticker  SampleTimer;                    // LA:  To Sample 1AI any 'x'ms

float   af_PlotSamples[240];            // LA:  "Horiz" Plot Array

uint16_t    aui16_PlotSamples[240];
uint16_t    aui16_PlotClears_Lo[240];
uint16_t    aui16_PlotClears_Hi[240];

int32_t     ai32_POS2VelGraph[4000];
uint16_t    aui16_PlotPOS2VelSamples[240];
uint16_t    aui16_PlotPOS2VelClears_Lo[240];
uint16_t    aui16_PlotPOS2VelClears_Hi[240];

int32_t     ai32_POS2AccGraph[4000];
int32_t     ai32_POS2JrkGraph[4000];

// LA:  MotionHandler
// LA:  MotionTimer =
//      =============
//
static  void    MotionHandler   (void);
Ticker  MotionTimer;                    // LA:  Gestisce il rilevamento (RT) di Velocità, Accelerazione e Jerk
                                        //      Esegue il Movimento Programmato

float   f_PWMPercent;                   //      Deprecated

float   fVelocity;
float   fAcceleration;
float   fJerk;
float   fTorque;

int64_t i64_Position_Prec;
int32_t i32_Velocity;
int32_t i32_Velocity_Prec;
int32_t i32_Acceleration;
int32_t i32_Acceleration_Prec;
int32_t i32_Jerk;

// LA:  Motion
//      ======
// LA:  Theory of Operation
//      ===================
//
//  Il PWM funziona da sè in Interrupt
//  Il QEI funziona da sè in Interrupt
//  Se si creano dei Ticker (Che sono a loro volta interrupt(s)) è possibile che PWM e QEI perdano correlazione con l'HW.
//
//  PQM
//
//  Il rinfresco del Display e la gestione del motion vanno fatte il più frequentemente possibile ma fuori dal loop dei Ticker.
//  Con qst versione (LA0005, che termina un FORK (il successivo è LA0010) quanto detto sopra è FUNZIONANTE.
//  Questo messaggio è incluso nel "commitment"

/*!
 * \brief Define IO for Unused Pin
 */
//DigitalOut  F_CS    (D6);               // MBED description of pin
//DigitalOut  SD_CS   (D8);               // MBED description of pin

DigitalIn   userButton  (USER_BUTTON);
//
DigitalOut  rENA_Off    (PC_0);           // CN7.38 -    Power Enable Relay,            Power Disabled when true
DigitalOut  rDIR_FWD    (PC_1);           // CN7.36 -    Move Direction Relay Bridge,   Move FW(Extends) when true

AnalogIn    adc_temp    (ADC_TEMP);
AnalogIn    adc_vref    (ADC_VREF);
AnalogIn    adc_vbat    (ADC_VBAT);

AnalogIn    ADC12_IN9   (PA_4);             // STM32 PA4
AnalogIn    ADC12_IN15  (PB_0);             // STM32 PB0

//  PWM
//  ===
//
PwmOut PWM_PB3(PWM_OUT);       // LA:  PWM_OUT = D3 = PB_3

//  QEI
//  ===
//
QEI Stabilus322699  (PA_1, PA_0, NC, 100, QEI::X4_ENCODING);
//DigitalIn   Hall_A  (PA_1);
//DigitalIn   Hall_B  (PA_0);

//  Motion
//  ======
//
//Ticker POS_MotionScan;                              // LA:    Non uso un Ticker. Agisce sotto Interrupt e falsa la lettura QEI e la sincronicità del PWM
//
in_sPosizionatoreSW    in_PosizionatoreSW;
out_sPosizionatoreSW   out_PosizionatoreSW;

//  LCD Display
//  ===========
//
//Ticker LCD_RefreshViews;                            // LA:    Non uso un Ticker. Agisce sotto Interrupt e falsa la lettura QEI e la sincronicità del PWM

void    FactoryReset    (void) {
    EepromFactoryReset( );
    HAL_NVIC_SystemReset( );
}

//  =======
//  =======
//  Main(s)
//  =======
//  =======
//
int main    (void){

    rDIR_FWD =  true;   // LA:  Actuator Extends
    rENA_Off =  true;   // LA:  Drive Power is Off
    //
    in_PosizionatoreSW.b_ServoLock =    false;
    in_PosizionatoreSW.rtServoLock_Q =  false;

    EepromInit();       // LA:  Inizializza la EEProm
    TimersInit();       // LA:  Parte il Timer a 1ms

    // LA:  FactoryReset se "userButton" premuto all'avvio
    //
    if  (userButton == 0) {
        FactoryReset();
    }
    DisplayDriverInit();

//    SampleTimer.attach_us(&SampleAndStore, 250);            // LA:  Scope has its own times
    SampleTimer.attach(&SampleAndStore, cf_SCOPeriod_s);    // LA:  Avvia l'OscilloScopio con TimeBase x [s]
    MotionTimer.attach(&MotionHandler, cf_MOTPeriod_s);     // LA:  Avvia il Motion con TimeBase x [s]
    PWM_PB3.period(cf_PWMPeriod_s);                         // LA:  Avvia il PWM con TimeBase x [s]
    PWM_PB3.write((float) 0.0);                             //      Set to 0%

    // LA:  Motion (1st) Setup
    //
    in_PosizionatoreSW.b_AxisPowered =  true;
    in_PosizionatoreSW.b_ACPos_Homed =  true;
    in_PosizionatoreSW.i32_Max_Speed =  512;        // [ui/s]
    in_PosizionatoreSW.i32_ZeroSpeed =  0;          //

    //  POS Mode
    //  ========
    //
//    in_PosizionatoreSW.b_ServoLock =    true;
//    in_PosizionatoreSW.rtServoLock_Q =  false;
    //
    in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS;                 // [ui]
    in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses();     //
//    in_PosizionatoreSW.i64_AccelerationWindow = 32;                        // LA:  Spazio concesso all'accelerazione.
//    in_PosizionatoreSW.i64_DecelerationWindow = 256;                        //      Spazio concesso alla decelerazione, è prioritario rispetto all'accelerazione.
    in_PosizionatoreSW.i64_AccelerationWindow = 1024;                        // LA:  Spazio concesso all'accelerazione.
    in_PosizionatoreSW.i64_DecelerationWindow = 2048;                        //      Spazio concesso alla decelerazione, è prioritario rispetto all'accelerazione.
    in_PosizionatoreSW.i64_diToleranceWindow =  16;                          //      Finestra di Tolleranza
    //
//    in_PosizionatoreSW.f_MaximumSpeed_x100_FW = 16.0;                       // % of "i32_Max_Speed"
//    in_PosizionatoreSW.f_MaximumSpeed_x100_BW = 62.0;                       //
    in_PosizionatoreSW.f_MaximumSpeed_x100_FW = 26.0;                       // % of "i32_Max_Speed"
    in_PosizionatoreSW.f_MaximumSpeed_x100_BW = 100.0;                       //
//    in_PosizionatoreSW.f_ServoLockSpeed_x100_FW =   4.8;                    //      Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
//    in_PosizionatoreSW.f_ServoLockSpeed_x100_BW =   18.0;                   //      Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]
    in_PosizionatoreSW.f_ServoLockSpeed_x100_FW =   6.2;                    //      Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
//    in_PosizionatoreSW.f_ServoLockSpeed_x100_FW =   4.8;                    //      Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
    in_PosizionatoreSW.f_ServoLockSpeed_x100_BW =   24.0;                   //      Riferimento di velocità minima a cui (appena) si muove l'asse [verso BW]

    //  JOG Mode
    //  ========
    //
    in_PosizionatoreSW.b_JogMode =  false;
    in_PosizionatoreSW.b_JogFW =    false;
    in_PosizionatoreSW.b_JogBW =    false;
    in_PosizionatoreSW.i32_JogAccel_ms =    500;    // [ms]
    in_PosizionatoreSW.i32_JogDecel_ms =    250;    //
    //
    in_PosizionatoreSW.f_JogSpeed_x100_FW = (in_PosizionatoreSW.f_MaximumSpeed_x100_FW/ 2);     // LA:  JOG's the Half of Max POS's Speed
    in_PosizionatoreSW.f_JogSpeed_x100_BW = (in_PosizionatoreSW.f_MaximumSpeed_x100_BW/ 2);     //

    //  Velocity Loop PID
    //  =================
    //
    //  Input Speed (ref)=  0.. 512[ui/s]
    PID_VelocityClosedLoop.setInputLimits(0.0f, (float)in_PosizionatoreSW.i32_Max_Speed);
    //  Output PWM  (ref)=  0.. 1
    PID_VelocityClosedLoop.setOutputLimits(0.0f, 1.0f);

    //  If there's a bias.
//    PID_VelocityClosedLoop.setBias(0.3);
    PID_VelocityClosedLoop.setMode(AUTO_MODE);
    //  Starts @Zero
    PID_VelocityClosedLoop.setSetPoint(0.0f);

    // LA:  Color RGB Component(s)
    //      ======================
    //
    //  RED     0000 1000 0000 0000     min     0x0800  02048
    //          1111 1000 0000 0000     max     0xf800  63488
    //
    //  GREEN   0000 0000 0010 0000     min     0x0020  00032
    //          0000 0111 1110 0000     max     0x07e0  02016
    //
    //  BLUE    0000 0000 0000 0001     min     0x0001  00001
    //          0000 0000 0001 1111     max     0x001f  00031
    //
    //  La componente ROSSA ha  5 bit di escursione (0.. 31),
    //  La componente VERDE ha  6 bit di escursione (0.. 63),
    //  La componente BLU ha    5 bit di escursione (0.. 31),
    //
    //  Le componenti RGB di "Color" sono quindi scritte negli appropriati registri come segue:
    //
    //  writeReg(RED,   (Color & 0xf800) >> 11);
    //  writeReg(GREEN, (Color & 0x07e0) >> 5);
    //  writeReg(BLUE,  (Color & 0x001f));
    //
    LCM_SetTextColor(Scale2RGBColor  (0, 0, 0), Scale2RGBColor  (31, 0, 0));    // LA:  Red on Black
    LCM_ClearScreen (Scale2RGBColor  (0, 0, 0));                                //      Black Background
    LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 0), "You Start Me Up ...");       //      Intro Text

//    rDIR_FWD =  false;      // Collapse
//    rENA_Off =  false;      // Power On

    in_PosizionatoreSW.b_ServoLock =    true;
    in_PosizionatoreSW.rtServoLock_Q =  true;

    while   (1) {

        // LA:  Scope, Theory of operation.
        //      ===========================
        //
        //  1)  Sample a Value @ any Step
        //  2)  Store @ the correct ms
        //  3)  Plot the current Section of the Sampling Vector
        //
        LCM_ShowTactics (
                        Stabilus322699.getPulses(),         // Row  1

                        out_PosizionatoreSW.i32_ATVSpeed,   //      3
                        adc_temp.read(),                    //      4
                        adc_vbat.read(),                    //      5
                        adc_vref.read(),                    //      6

                        ADC12_IN9.read(),                   //      8
                        ADC12_IN15.read(),                  //      9
                        (f_PWMPercent* 100),                //      10

                        i32_Velocity,                       //      11
                        i32_Acceleration,                   //      12
                        i32_Jerk                            //      13

                        );

        LCM_PlotScope  (
                        Scale2RGBColor  (0, 0, 0),          //  Back:   Black
                        Scale2RGBColor  (31, 0, 0)          //  Fore:   Red
                        );

        LCM_PlotSpeed  (
                        Scale2RGBColor  (0, 0, 0),          //  Back:   Black
                        Scale2RGBColor  (31, 0, 0)          //  Fore:   Red
                        );

        if  (out_PosizionatoreSW.b_InPosition)
            if  (in_PosizionatoreSW.i64_TargetPosition > 0)
                in_PosizionatoreSW.i64_TargetPosition = 0;
            else
                in_PosizionatoreSW.i64_TargetPosition = ci64_TargetPOS;
    }
}

void    LCM_ShowTactics(
                        int64_t i64_Pulses,
                        int32_t i32_ATVSpeed,
                        //
                        float   f_ai0000_Aux,
                        float   f_ai0001_Aux,
                        float   f_ai0002_Aux,
                        float   f_ai0003_Aux,
                        float   f_ai0004_Aux,
                        float   f_ai0005_Aux,
                        //
                        int32_t i32_Velocity,
                        int32_t i32_Acceleration,
                        int32_t i32_Jerk
                        ) {

char    StringText[MAX_CHAR_PER_LINE+ 1];  // don't forget the /0 (end of string)

static int64_t  i64_Pulses_Prec;
static uint32_t ms_0002_prec;

static float    f_ai0000_prec;
static float    f_ai0001_prec;
static float    f_ai0002_prec;
static float    f_ai0003_prec;
static float    f_ai0004_prec;
static float    f_ai0005_prec;

static uint32_t i32_Velocity_prec;
static uint32_t i32_Acceleration_prec;
static uint32_t i32_Jerk_prec;

    if  (i64_Pulses !=  i64_Pulses_Prec) {
        sprintf (StringText,
                "Pulses: %d     ", (int32_t)i64_Pulses);
        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 1), StringText);
        i64_Pulses_Prec =   i64_Pulses;
    }

/*
    if  (i32_ATVSpeed != ms_0002_prec) {
        sprintf (StringText,
                "Speed[ui]: %d     ", i32_ATVSpeed);
        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 3), StringText);
        ms_0002_prec = i32_ATVSpeed;
    }

    if  (f_ai0000_Aux != f_ai0000_prec) {
        sprintf (StringText,
                "ADC Temp = %f ", (f_ai0000_Aux* 100));
        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 4), StringText);
        f_ai0000_prec = f_ai0000_Aux;
    }

    if  (f_ai0001_Aux != f_ai0001_prec) {
        sprintf (StringText,
                "ADC VBat = %f ", (f_ai0001_Aux* 10));
        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
        f_ai0001_prec = f_ai0001_Aux;
    }

    if  (f_ai0002_Aux != f_ai0002_prec) {
        sprintf (StringText,
                "ADC VRef = %f ", (f_ai0002_Aux* 10));
        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 6), StringText);
        f_ai0002_prec = f_ai0002_Aux;
    }

    if  (f_ai0003_Aux != f_ai0003_prec) {
        sprintf (StringText,
                "ADC12.09 = %f ", (f_ai0003_Aux* 10)/ 3);
        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 8), StringText);
        f_ai0003_prec = f_ai0003_Aux;
    }

    if  (f_ai0004_Aux != f_ai0004_prec) {
        sprintf (StringText,
                "ADC12.15 = %f ", (f_ai0004_Aux* 10)/ 3);
        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 9), StringText);
        f_ai0004_prec = f_ai0004_Aux;
    }

    if  (f_ai0005_Aux != f_ai0005_prec) {
        sprintf (StringText,
                "PB3 PWM%% = %f ", (f_ai0005_Aux));
        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 10), StringText);
        f_ai0005_prec = f_ai0005_Aux;
    }
*/
    if  (i32_Velocity != i32_Velocity_prec) {
        sprintf (StringText,
                "Vel[ui/10ms]:   %d ", i32_Velocity);                   //, fVelocity);
//        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 11), StringText);
        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 3), StringText);
        i32_Velocity_prec = i32_Velocity;
    }
    if  (i32_Acceleration != i32_Acceleration_prec) {
        sprintf (StringText,
                "Acc[ui/10ms^2]: %d ", i32_Acceleration);               //, fAcceleration);
//        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 12), StringText);
        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 4), StringText);
        i32_Acceleration_prec = i32_Acceleration;
    }
    if  (i32_Jerk != i32_Jerk_prec) {
        sprintf (StringText,
                "Jerk:           %d ", i32_Jerk);                       //, fJerk);
//        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 13), StringText);
        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
        i32_Jerk_prec = i32_Jerk;
    }
}

static void SampleAndStore  (void) {
int16_t i16_SampleIndex;
int64_t i64_SampleIndex;

//    af_PlotSamples[240- 1] =   ADC12_IN9.read();
    af_PlotSamples[240- 1] =   (float)  Stabilus322699.getChannelA() * 0.33f;

    for (i16_SampleIndex = 0; i16_SampleIndex < (240- 1); i16_SampleIndex++)
        af_PlotSamples[i16_SampleIndex] =   af_PlotSamples[i16_SampleIndex+ 1];

    // LA:  Position's Graph Section
    //      ========================
    //
    i64_SampleIndex =   Stabilus322699.getPulses();
    ai32_POS2VelGraph[i64_SampleIndex] =  i32_Velocity;
    ai32_POS2AccGraph[i64_SampleIndex] =  i32_Acceleration;
    ai32_POS2JrkGraph[i64_SampleIndex] =  i32_Jerk;
}

static void MotionHandler   (void) {
static int16_t  i16_Index = 0;

    // LA:  Retrieve Actual Position
    //
    in_PosizionatoreSW.i64_ActualPosition = Stabilus322699.getPulses();

    // LA:  Execute Motion
    //
    PosizionatoreSW (in_PosizionatoreSW, out_PosizionatoreSW);
    in_PosizionatoreSW.rtServoLock_Q =  false;

    // LA:  Handle PostServo
    //

    //    int64_t   i64_StartPosition;
    //    int64_t   i64_Distance;
    //    bool   b_Accelerating;                   // LA:  bACPos_Accelerating
    //    bool   b_MaxSpeedReached;
    //    bool   b_Decelerating;                   //      bACPos_Decelerating
    //    bool   b_InPosition;
    //    bool   b_InToleranceFW;
    //    bool   b_InToleranceBW;
    
    //    int32_t    i32_ATVSpeed;
//    f_PWMPercent =  ((float)out_PosizionatoreSW.i32_ATVSpeed)/ (float)in_PosizionatoreSW.i32_Max_Speed;     // LA:  In Range (float) 0.. 1
//    PWM_PB3.write((float) 1.0- f_PWMPercent);                                                               //      Set to x%

    //    bool   b_ATVDirectionFW;
    rDIR_FWD =  out_PosizionatoreSW.b_ATVDirectionFW;
    
    //    bool   b_ATVDirectionBW;
    
    //    bool   b_STW1_On;
    //    bool   b_STW1_NoCStop;
    //    bool   b_STW1_NoQStop;
    //    bool   b_STW1_Enable;
    rENA_Off =  !out_PosizionatoreSW.b_STW1_Enable;

    // LA:  Update Motion Dynamic References
    //      ================================
    //
    //  Per avere maggiore granularità delle misure, l'acquisizione viene fatta ogni 10 interventi
    //  Se il motion gira a 10ms, i riferimenti dinamici saranno calcolati ogni 100
    //
//    if  (i16_Index == 0) {
    static uint32_t ui32_PreviousStep_ms;
    uint32_t    ui32_ActualStepSampled_ms;
    uint32_t    ui32_PassedActual_ms;
    //
    float   fPassedActual_sxs;

        // LA:  Generazione del millisecondo Attuale
        //      ====================================
        //
        //  Invoca il timer di sistema (TimersTimerValue) e lo confronta col suo precedente.
        //  Una volta elaborato e "scevrato" l'eventuale "Rollover" la sezione ritorna "ui32_PassedActual_ms_Local".
        //  "ui32_PassedActual_ms_Local" rappresenta i [ms] passati tra una istanza e l'altra
        // 
        ui32_ActualStepSampled_ms = TimersTimerValue();                                         //  Freezes the Actual Sample.
        if  (ui32_ActualStepSampled_ms >=   ui32_PreviousStep_ms)
            ui32_PassedActual_ms =  (ui32_ActualStepSampled_ms- ui32_PreviousStep_ms);              //  Result =>   Actual- Previous
        else
            ui32_PassedActual_ms =  ui32_ActualStepSampled_ms+ (0x7fffffff- ui32_PreviousStep_ms);  //  Result =>   Actual+ (Rollover- Previous)
        ui32_PreviousStep_ms =  ui32_ActualStepSampled_ms;                                          //  Store(s)&Hold(s) actual msSample
        fPassedActual_sxs = ((float) 1000.0/ (float) ui32_PassedActual_ms);                         //  Steps Any [s]

        i32_Velocity =  (int32_t) (in_PosizionatoreSW.i64_ActualPosition- i64_Position_Prec);   // LA:  Velocity in [ui/10ms]
        i64_Position_Prec = in_PosizionatoreSW.i64_ActualPosition;
        i32_Acceleration =  (i32_Velocity- i32_Velocity_Prec);                                  // LA:  Acceleration in [ui/10ms^2]
        i32_Velocity_Prec = i32_Velocity;
        i32_Jerk =  (i32_Acceleration- i32_Acceleration_Prec);                                  // LA:  Jerk
        i32_Acceleration_Prec = i32_Acceleration;

        fVelocity = (float) i32_Velocity * fPassedActual_sxs;           //  Velocity in [ui/s]
        fAcceleration = (float) i32_Acceleration * fPassedActual_sxs;   //  Acceleration in [ui/s^2]
        fJerk = (float) i32_Jerk * fPassedActual_sxs;                   //  Jerk

        // LA:  PID Compute Section
        //      ===================
        //

        //  Update the process variable.
        PID_VelocityClosedLoop.setProcessValue((float)i32_Velocity);

        //  Set Desired Value
        PID_VelocityClosedLoop.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed);

        //  Release a new output.
//        f_PWMPercent =  ((float)out_PosizionatoreSW.i32_ATVSpeed)/ (float)in_PosizionatoreSW.i32_Max_Speed;     // LA:  In Range (float) 0.. 1
        f_PWMPercent =  PID_VelocityClosedLoop.compute();
        PWM_PB3.write((float) 1.0- f_PWMPercent);                                                               //      Set to x%
//    }
//    i16_Index++;
//    if  (i16_Index >= 10)
//        i16_Index = 0;

/*
    // LA:  Position's Graph Section
    //      ========================
    //
    ai32_POS2VelGraph[in_PosizionatoreSW.i64_ActualPosition] =  i32_Velocity;
    ai32_POS2AccGraph[in_PosizionatoreSW.i64_ActualPosition] =  i32_Acceleration;
    ai32_POS2JrkGraph[in_PosizionatoreSW.i64_ActualPosition] =  i32_Jerk;
*/
}

/*
#include "PID.h"

#define RATE 0.1

//Kc, Ti, Td, interval
PID controller(1.0, 0.0, 0.0, RATE);
AnalogIn pv(p15);
PwmOut   co(p26);

int main(){

  //Analog input from 0.0 to 3.3V
  controller.setInputLimits(0.0, 3.3);
  //Pwm output from 0.0 to 1.0
  controller.setOutputLimits(0.0, 1.0);
  //If there's a bias.
  controller.setBias(0.3);
  controller.setMode(AUTO_MODE);
  //We want the process variable to be 1.7V
  controller.setSetPoint(1.7);

  while(1){
    //Update the process variable.
    controller.setProcessValue(pv.read());
    //Set the new output.
    co = controller.compute();
    //Wait for another loop calculation.
    wait(RATE);
  }

}
*/