/************************************************************************************************************************ * 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; /******************************************************************************************** * 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= 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); /*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(); }