File content as of revision 36:cab8aa44ef91:

//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.
//
// LA:  11 Aprile 2022, Aggiungo il PID
//      ===============================
//
//  La retroazione sulla Velocità (Calcolata ogni "cf_MOTPeriod_s") è eseguita, nello stesso intervallo, da "PID_VelocityClosedLoop"
//  Il parametro di Riferimento è la Velocity Calcolata
//  Il Fattore di correzione è il PWM in uscita
//

#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_FW (0.1, 0.001, 0.01, RATE);

PID PID_VelocityClosedLoop_FW (0.4, 0.0, 0.0001, RATE);
PID PID_VelocityClosedLoop_BW (0.41, 0.0, 0.01, 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 =  20;        // [ui/ms]
    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 = 512;                        // LA:  Spazio concesso all'accelerazione.
    in_PosizionatoreSW.i64_DecelerationWindow = 1024;                        //      Spazio concesso alla decelerazione, è prioritario rispetto all'accelerazione.
    in_PosizionatoreSW.i64_diToleranceWindow =  16;                         //      Finestra di Tolleranza
    //
    in_PosizionatoreSW.f_MaximumSpeed_x100_FW = 100.0f;                      // % of "i32_Max_Speed"
    in_PosizionatoreSW.f_MaximumSpeed_x100_BW = 100.0f;                      //
    in_PosizionatoreSW.f_ServoLockSpeed_x100_FW =   25.0f;                   //      Riferimento di velocità minima a cui (appena) si muove l'asse [verso FW]
    in_PosizionatoreSW.f_ServoLockSpeed_x100_BW =   50.0f;                   //      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_FW.setInputLimits(0.0f, (float)in_PosizionatoreSW.i32_Max_Speed);
    PID_VelocityClosedLoop_BW.setInputLimits(0.0f, (float)in_PosizionatoreSW.i32_Max_Speed);
    //  Output PWM  (ref)=  0.. 1
    PID_VelocityClosedLoop_FW.setOutputLimits(0.0f, 1.0f);
    PID_VelocityClosedLoop_BW.setOutputLimits(0.0f, 1.0f);

    //  If there's a bias.
//    PID_VelocityClosedLoop.setBias(0.3);
//    PID_VelocityClosedLoop.setMode(AUTO_MODE);
    PID_VelocityClosedLoop_FW.setMode(MANUAL_MODE);
    PID_VelocityClosedLoop_BW.setMode(MANUAL_MODE);

    // 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_Velocity != i32_Velocity_prec) {
        sprintf (StringText,
                "Velocity[ui/ms]: %d ", i32_Velocity);                   //, fVelocity);
        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 3), StringText);
        i32_Velocity_prec = i32_Velocity;
    }

    if  (out_PosizionatoreSW.i32_ATVSpeed != i32_Acceleration_prec) {
        sprintf (StringText,
                "ATVSpeed[ui/ms]: %d ", out_PosizionatoreSW.i32_ATVSpeed);               //, fAcceleration);
        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 4), StringText);
        i32_Acceleration_prec = out_PosizionatoreSW.i32_ATVSpeed;
    }
    if  (f_PWMPercent != f_ai0005_prec) {
        sprintf (StringText,
                "PID_FB Compute:  %f ", f_PWMPercent);                       //, fJerk);
        LCM_DrawString  (0, 0+ (TEXT_ROW_SPACING* 5), StringText);
        f_ai0005_prec = f_PWMPercent;
    }

/*
    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;
static uint32_t ui32_PreviousStep_ms;
uint32_t    ui32_ActualStepSampled_ms;
uint32_t    ui32_PassedActual_ms;
//
float   fPassedActual_sxs;

    // 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
    //      ================================

    // 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/ms]
    i64_Position_Prec = in_PosizionatoreSW.i64_ActualPosition;
    i32_Acceleration =  (i32_Velocity- i32_Velocity_Prec);                                  // LA:  Acceleration in [ui/ms^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
    //      ===================
    //
    if  (out_PosizionatoreSW.b_ATVDirectionFW) {
        PID_VelocityClosedLoop_BW.reset();

        //  Update the process variable.
        PID_VelocityClosedLoop_FW.setProcessValue((float)i32_Velocity);
        //  Set Desired Value
        PID_VelocityClosedLoop_FW.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed);
        //  Release a new output.
        f_PWMPercent =  PID_VelocityClosedLoop_FW.compute();
    }
    else {
        PID_VelocityClosedLoop_FW.reset();

        //  Update the process variable.
        PID_VelocityClosedLoop_BW.setProcessValue((float)i32_Velocity);
        //  Set Desired Value
        PID_VelocityClosedLoop_BW.setSetPoint((float)out_PosizionatoreSW.i32_ATVSpeed);
        //  Release a new output.
        f_PWMPercent =  PID_VelocityClosedLoop_BW.compute();
    }
    PWM_PB3.write((float) 1.0- f_PWMPercent);                                                               //      Set to x%
}