diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c | 326 |
1 files changed, 326 insertions, 0 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c new file mode 100644 index 000000000..10baaed8e --- /dev/null +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -0,0 +1,326 @@ +/************************************************************************************************************************ + * 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" +////////////////////////////////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; + +/******************************************************************************************** +* 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(DeviceId2Motor[Motor_i], ThreadControlCBFunction, eOneMillisecond); + } + //set 3 dancers to the profile positions + return NextState; +} + +//******************************************************************************************************************** +static ReturnCode PreSegmentState(void *JobDetails) +{ + + int Motor_i; + for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++) + { + MotorControlConfig[Motor_i].m_SetParam = getMotorFreq(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); + + /*if ((_motorId == SCREW_MOTOR)&&(pause_active)) + { + tick = UsersysTickGet(); + len = usnprintf(str, 60, "\r\n PrintingIterate tick %d state %d retcode %d ",tick, gPrintingState[_motorId],returnCode); + cb_push_back (str, len); + //SendInterruptMessageToHost(10+gPrintingState[_motorId],returnCode); + }*/ + gPrintingState = LookupTransitions(gPrintingState, returnCode); + if (keepstate != gPrintingState){ + tick = UsersysTickGet(); + len = usnprintf(str, 60, "\r\n changed state tick %d state %d retcode %d ", tick, gPrintingState,returnCode); + cb_push_back (str, len); + + } + + // + // 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(); +} |