path: root/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c

/************************************************************************************************************************
 * Thread_print.c
 * Printing module is responsible for :
     * operating diffrent winding algorithms with predefined parameters from the UI
     * operating the dispensers according to predefined dispensing rate from the UI
 **************************************************************************************************************************/
#include "include.h"
#include "thread.h"
#include "../control/control.h"
#include "../control/pidalgo.h"
#include "PMR/Hardware/HardwareMotor.pb-c.h"
#include "PMR/Hardware/HardwareMotorType.pb-c.h"
#include "drivers/Motors/Motor.h"
#include "drivers/Heater/TemperatureSensor.h"
#include "drivers/Heater/Heater.h"
////////////////////////////////State machine operation////////////////////////////////////
//the state machine operation is used to operate in runtime correct profile flow execution
//by recieved esign flow of the user from the UI
///////////////////////////////////////////////////////////////////////////////////////////
typedef enum
{
    NextState = 0,
    Repeat,
    Inter,
    Home,
    Stop
} ReturnCode;


uint32_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {MOTOR_RDRIVING,MOTOR_DRYER_DRIVING,MOTOR_LDRIVING,MOTOR_WINDER,MOTOR_SCREW};

/********************************************************************************************
* functions describes motor operation flow and movement state during profile execution
* used to operate in runtime correct profileflow execution
*********************************************************************************************/
static ReturnCode EntryState(void *JobDetails);
static ReturnCode PrepareState(void *JobDetails);
static ReturnCode PreSegmentState(void *JobDetails);
static ReturnCode SegmentState(void *JobDetails);
static ReturnCode EndState(void *JobDetails);
static ReturnCode ExitState(void *JobDetails);
;

/**********************************************************************
* the array and enum of PrintingState_t below must be in sync order
***********************************************************************/
static ReturnCode (* state[])(void *JobDetails) = { EntryState, PrepareState, PreSegmentState,  SegmentState, EndState, ExitState};

typedef enum
{
    Entry= 0,
    Prepare,
    PreSegment,
    Segment,
    End,
    Exit
} PrintingState_t;

typedef struct
{
    PrintingState_t m_sourceState;
    ReturnCode          m_returnCode;
    PrintingState_t     m_destinationState;
} Transition_t;

//*************************************************************
/* transitions from end state aren't needed */
//*************************************************************
#define NUM_OF_TRANSITION   17
#define EXIT_STATE          Exit
#define ENTRY_STATE         Entry
/*************************************************************
 * table which describes fast motors transitions states
 * during p_profile / segments execution
 *************************************************************/
static Transition_t stateTransitionTable[NUM_OF_TRANSITION] =
{};
/*    {Entry,         NextState,  HomingStart},
    {Entry,         Repeat,     Entry},         //for homing of dispensers
    {HomingStart,   NextState,  Start},
    {HomingStart,   Repeat,     HomingStart},
    {Start,         NextState,  Segment},
    {Start,         Repeat,     Start},
    {Segment,       Inter,      Intersegment},
    {Segment,       Repeat,     Segment},
    {Segment,       Home,       HomingEnd},
    {Intersegment,  NextState,  Segment},
    {Intersegment,  Repeat,     Intersegment},
    {Intersegment,  Home,       HomingEnd},
    {HomingEnd,     NextState,  End},
    {HomingEnd,     Repeat,     HomingEnd},
    {End,           NextState,  Entry},
    {End,           Repeat,     Entry},
    {Exit,          Stop,       Exit}           //for stoping the machine iteration in case of error
};*/
typedef struct
{
    bool                m_isEnabled;
    uint32_t            m_SetParam;
    uint32_t            m_mesuredParam;
    float               m_preError;
    float               m_integral;
    float               m_calculatedError;
    bool                m_isReady;
    PID_Config_Params   m_params;
}MotorControlConfig_t;
/*typedef struct
{
    float epsilon;
    float dt;
    float MAX;
    float MIN;
    float Kp;
    float Kd;
    float Ki;
}PID_Config_Params;
#define epsilon 0.01
#define dt 0.01 //100ms loop time
#define MAX 4 //For Current Saturation
#define MIN -4
#define Kp 0.1
#define Kd 0.01
#define Ki 0.005
*/
MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM];
////////////////////////Slow Motor State////////////////////////////////////
static PrintingState_t gPrintingState;
////////////////////////////////////////////////////////////////////////////

uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue)
{
    int i,index=MAX_THREAD_MOTORS_NUM;
    for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
        if (DeviceId2Motor[i] == deviceID)
        {
            index = i;
            break;
        }
    if (index==MAX_THREAD_MOTORS_NUM)
    {
        LOG_ERROR (deviceID, "No motor  for device");
        return 0xFFFFFFFF;
    }
    if(MotorControlConfig[index].m_isEnabled && (MotorControlConfig[index].m_SetParam != 0))
    {
        MotorControlConfig[index].m_mesuredParam = ReadValue;
        MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation(MotorControlConfig[index].m_SetParam , MotorControlConfig[index].m_mesuredParam,
                                                                              &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
        if (MotorControlConfig[index].m_calculatedError >= MotorControlConfig[index].m_params.MAX)
        {
            MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MAX;
        }
        if (MotorControlConfig[index].m_calculatedError < MotorControlConfig[index].m_params.MIN)
        {
            MotorControlConfig[index].m_calculatedError = MotorControlConfig[index].m_params.MIN;
        }

        //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
    }

 return OK;
}

//********************************************************************************************************************
/********************************************************************************************************************
*function describes entry point of motor in profile execution - accelerate from stop position
*function described above used to operate motor operation flow and movement state during profile execution
*********************************************************************************************************************/
static ReturnCode EntryState(void *JobDetails)
{
    return NextState;
}

//********************************************************************************************************************
static ReturnCode PrepareState(void *JobDetails)
{
    int Motor_i;
    //start thread control for all motors
    for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
    {
        MotorControlConfig[Motor_i].m_params.MAX = MotorsCfg[Motor_i].maxfreq;
        MotorControlConfig[Motor_i].m_params.MIN = MotorsCfg[Motor_i].minfreq;
        MotorControlConfig[Motor_i].m_params.Kd = MotorsCfg[Motor_i].kd;
        MotorControlConfig[Motor_i].m_params.Kp = MotorsCfg[Motor_i].kp;
        MotorControlConfig[Motor_i].m_params.Ki = MotorsCfg[Motor_i].ki;
        MotorControlConfig[Motor_i].m_params.dt = eOneMillisecond;
        MotorControlConfig[Motor_i].m_calculatedError = 0;
        MotorControlConfig[Motor_i].m_integral = 0;
        MotorControlConfig[Motor_i].m_isEnabled = true;
        MotorControlConfig[Motor_i].m_isReady = true;
        MotorControlConfig[Motor_i].m_mesuredParam = 0;
        MotorControlConfig[Motor_i].m_preError = 0;
        MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage
        AddControlCallback(ThreadControlCBFunction, eOneMillisecond,TemplateDataReadCBFunction,Motor_i,0);
    }

    //set 3 dancers to the profile positions
    return NextState;
}

//********************************************************************************************************************
static ReturnCode PreSegmentState(void *JobDetails)
{

    TimerMotors_t Motor_i;
    for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
    {
        MotorControlConfig[Motor_i].m_SetParam = MotorGetSpeed(getMotorId(Motor_i));//need to update SetParams on presegment stage
    }
    // set the new speed in the dryer motor to the speed of the new segment
    // activate control fr all motors
    //set speed for both rocker motors
    //wait for all motors to get to the required speed (set the target speed for the control to check)
    //call the job state machine when the thread system is ready
    return NextState;
}

//********************************************************************************************************************
static ReturnCode SegmentState(void *JobDetails)
{
    return Repeat;
}

//********************************************************************************************************************
static ReturnCode EndState(void *JobDetails)
{
    return NextState;
}
//********************************************************************************************************************
static ReturnCode ExitState(void *JobDetails)
{
    return Stop;
}


//***********************************************************************************************************************
//this function is responsible for operating and transitioning between the diffrent motor state executions of the profile
//the lower managment level
//***********************************************************************************************************************
static PrintingState_t LookupTransitions(PrintingState_t state,ReturnCode returnCode)
{
    char str[80];
    uint8_t len = 0;
    uint8_t indexInTransitionTable;
    for (indexInTransitionTable = 0; indexInTransitionTable < NUM_OF_TRANSITION; ++indexInTransitionTable)
    {
        if ((stateTransitionTable[indexInTransitionTable].m_sourceState == state) && (stateTransitionTable[indexInTransitionTable].m_returnCode == returnCode))
        {
            //len = usnprintf(str, 60, "\r\n  tick %d state %d return code %d",tick,state, returnCode );
            //cb_push_back (str, len);

            //in normal execution flow function should not arrive here
            //in case it did the meaning is that the entery point was wrong and a bug should be corrected
            return stateTransitionTable[indexInTransitionTable].m_destinationState;
        }
    }
    //int tick = UsersysTickGet();
    //len = usnprintf(str, 60, "\r\n  tick %d state %d return code %d",tick,state, returnCode );
    //cb_push_back (str, len);

    //in normal execution flow function should not arrive here
    //in case it did the meaning is that the entery point was wrong and a bug should be corrected
    len = usnprintf(str, 80, "Internal: invalid slow motor transition state %d return code %d",state, returnCode );

    return EXIT_STATE;
}
//********************************************************************************
//this function is used to manage and operate the motor managmant state mashine
//the highest managment level
//********************************************************************************
bool ThreadPrintingIterate(void *JobDetails)
{
    uint32_t tick = 0;
    char str[60];
    uint8_t len = 0;
    PrintingState_t keepstate  =  gPrintingState;
    //
    // Disable all interrupts.
    //
    ROM_IntMasterDisable();

    ReturnCode (* state_fun)(void *JobDetails) = state[gPrintingState];
    //if (_motorId == SCREW_MOTOR)
    //    screw_movement[gPrintingState[_motorId]]++;
    ReturnCode returnCode = state_fun(JobDetails);

    gPrintingState = LookupTransitions(gPrintingState, returnCode);
//    if (keepstate != gPrintingState){
//    }

    //
    // Enable all interrupts.
    //
    ROM_IntMasterEnable();
    return (gPrintingState != EXIT_STATE);
}


//********************************************************************************************************************

void ThreadPrintingsInit(void)
{
//        gPrintingState = Start;
}

//********************************************************************************************************************

void ThreadStartPrinting(void)
{
    gPrintingState = ENTRY_STATE;
    //PrintingIterate();
}

//********************************************************************************************************************
//********************************************************************************************************************

void ThreadStopPrinting(void)
{
    gPrintingState = EXIT_STATE;
    //PrintingIterate();
}