/************************************************************************************************************************ * 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 "PMR/Printing/JobSegment.pb-c.h" #include "PMR/Printing/JobTicket.pb-c.h" #include "StateMachines/Printing/PrintingSTM.h" #include "drivers/Motors/Motor.h" #include "drivers/Danser_SSI/ssi_comm.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 /////////////////////////////////////////////////////////////////////////////////////////// uint32_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {MOTOR_RDRIVING,MOTOR_DRYER_DRIVING,MOTOR_LDRIVING,MOTOR_WINDER,MOTOR_SCREW}; uint32_t ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS}; 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; MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM]; uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM]; ////////////////////////Slow Motor State//////////////////////////////////// uint32_t ThreadPreSegmentState(void *JobDetails); //////////////////////////////////////////////////////////////////////////// uint32_t ThreadSpeedControlCBFunction(uint32_t deviceID, uint32_t ReadValue) { //read value is the dancer angle 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; } uint32_t ThreadControlCBFunction(uint32_t deviceID, uint32_t ReadValue) { //read value is the dancer angle int i,index=MAX_THREAD_MOTORS_NUM; uint32_t TranslatedReadValue = ReadValue - 8740; 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; } //******************************************************************************************************************** //******************************************************************************************************************** #ifdef DEBUG_TEST_FUNCTIONS uint32_t Debug_Get_Dancer_Read(uint32_t DancerId, uint32_t Parameter1, uint32_t Parameter2) { return (rand() % (103 + 1 + 103) - 103); } #endif uint32_t ThreadInitialTestStub(HardwareMotor * request) { //MotorsConfigMessage(request); ThreadPrepareState(request); ThreadPreSegmentState(request); return OK; } //******************************************************************************************************************** uint32_t ThreadPrepareState(void *JobDetails) { int Motor_i, HW_Motor_Id; //start thread control for all motors for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; MotorControlConfig[Motor_i].m_params.MAX = MotorsCfg[HW_Motor_Id].maxfreq; MotorControlConfig[Motor_i].m_params.MIN = MotorsCfg[HW_Motor_Id].minfreq; MotorControlConfig[Motor_i].m_params.Kd = MotorsCfg[HW_Motor_Id].kd; MotorControlConfig[Motor_i].m_params.Kp = MotorsCfg[HW_Motor_Id].kp; MotorControlConfig[Motor_i].m_params.Ki = MotorsCfg[HW_Motor_Id].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 #ifdef DEBUG_TEST_FUNCTIONS if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],0); // continue; AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i); else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)) AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i); #else if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled continue; //AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,MotorGetSpeed,ThreadMotorIdToMotorId[Motor_i],Motor_i); else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)) AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i); #endif } Winder_Prepare(); //set 3 dancers to the profile positions ControlStart(); return OK; } //******************************************************************************************************************** uint32_t ThreadPreSegmentState(void *JobDetails) { //set the speed only before the first segment, speed is constant accros job JobTicket* JobTicket = JobDetails; // int process_speed = JobTicket->processparameters->dyeingspeed; int process_speed = 500; int dryer_speed = process_speed * MotorsCfg[MOTOR_DRYER_DRIVING].ratio2dryerspeed; //set dryer_speed_translation_here // set the new speed in the dryer motor to the speed of the new segment MotorSetSpeed(MOTOR_DRYER_DRIVING, process_speed, MotorsCfg[MOTOR_DRYER_DRIVING].minmicrostep); // 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 PreSegmentReady(Module_Thread,OK); return OK; } //******************************************************************************************************************** uint32_t ThreadSegmentState(void *JobDetails) { return OK; } //******************************************************************************************************************** uint32_t ThreadEndState(void *JobDetails) { int Motor_i; for ( Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++) { StopMotor(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); } return OK; } //******************************************************************************************************************** void ThreadStartPrinting(void) { //PrintingIterate(); } //******************************************************************************************************************** //******************************************************************************************************************** void ThreadStopPrinting(void) { //PrintingIterate(); }