/************************************************************************************************************************ * 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 "thread_ex.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/Hardware/HardwareDancerType.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" #include "drivers/Motors/Motor.h" #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h" #include "modules/heaters/heaters.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 CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0}; TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,HARDWARE_MOTOR_TYPE__MOTO_SCREW}; HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS}; uint32_t ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF}; uint32_t SpeedControlId=0xFF; double DancerError[NUM_OF_DANCERS] = {0.0}; int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0}; uint32_t JobCounter = 0; typedef struct { bool m_isEnabled; int32_t m_SetParam; float 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]; uint32_t PreviousPosition = 0, CurrentPosition = 0; double totalLength = 0.0; double CurrentRequestedLength = 0.0; double CurrentProcessedLength = 0.0; double LengthCalculationMultiplier; int CurrentSegmentId = 0; typedef void (* ProcessedLengthFunc)(void); ProcessedLengthFunc ProcessedLengthFuncPtr = NULL; // segment/intersegment/distance to spool finished void ThreadSegmentEnded(void); void ThreadInterSegmentEnded(void); void ThreadDistanceToSpoolEnded(void); double KeepNormalizedError = 0; ////////////////////////Slow Motor State//////////////////////////////////// //uint32_t ThreadPreSegmentState(void *JobDetails); //////////////////////////////////////////////////////////////////////////// /******************************************************************** * * Name : GTIME_Delta_Time_Pass * * Parameters : start_time. * * Return : time pass from start time * * Description : * *********************************************************************/ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read) { uint32_t Time_Pass; // #define MAX_COUNTER 0x3FFF //14 bits #define MAX_COUNTER 0x3FFFFF //22 bits if (Current_Read < Previous_Read) Time_Pass = (MAX_COUNTER - Previous_Read) + Current_Read + 1; else Time_Pass = Current_Read - Previous_Read; return (Time_Pass); } /***************************************************************************************** * * * * * * * **************************************************************************************/ uint32_t initialpos = 0xFFFF; void ThreadUpdateProcessLength (double length, void *Funcptr) { CurrentRequestedLength = length*100;//Centimetres CurrentProcessedLength = 0; //PreviousPosition = 0; //CurrentPosition = 0; totalLength = 0; ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr; initialpos = 0xFFFF; } double MotorSentData[1000] = {0}; uint32_t PosDif[1000] = {0}; int MotorDataIndex = 0; uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { uint32_t positionDiff = 0; double length = 0.0; int index = MAX_THREAD_MOTORS_NUM; if (IfIndex>>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; if (CurrentRequestedLength == 0.0) return OK; if (index != FEEDER_MOTOR) { LOG_ERROR (IfIndex, "Wrong Motor"); return 0xFFFFFFFF; } CurrentPosition = MotorGetPosition(ThreadMotorIdToMotorId[index]); if (CurrentPosition == 0) return OK; //unusable data if (initialpos == 0xFFFF) { PreviousPosition = CurrentPosition; initialpos = 0; } positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition); //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; PreviousPosition = CurrentPosition; // total length = (position diff / full cycle) * pulley perimeter //(positionDiff/pulseperround)*((2*PI*motor_Radius) //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; length = (double)(positionDiff)*LengthCalculationMultiplier; if (length > 0.1) { totalLength+=length; } //#warning control disabled CurrentProcessedLength+=length; PosDif[MotorDataIndex] = CurrentPosition; //PosDif[MotorDataIndex] = positionDiff; MotorSentData[MotorDataIndex] = length; MotorDataIndex+=1; if (MotorDataIndex == 999) MotorDataIndex = 0; static int pooler_counter = 0; pooler_counter++; if (pooler_counter%10 == 0) { SendJobProgress(CurrentProcessedLength/CurrentRequestedLength,CurrentSegmentId,false); //SendJobProgress(/*KeepNormalizedError*/MotorControlConfig[index].m_calculatedError,CurrentSegmentId,false); } if (pooler_counter>=100) { //HeatingTestSendResonse(0, false,true,true, MotorDriverRequest[22].Speed,MotorDriverRequest[18].Speed,MotorDriverRequest[15].Speed,MotorDriverRequest[3].Speed, "MotorSpeed"); HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/length,positionDiff/*(int)error_integered*/,CurrentProcessedLength,CurrentRequestedLength, "FeederLength"); pooler_counter = 0; } if (CurrentProcessedLength>=CurrentRequestedLength ) { // segment/intersegment/distance to spool finished if (ProcessedLengthFuncPtr) ProcessedLengthFuncPtr(); } return OK; } uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //read value is the dancer angle int index=MAX_THREAD_MOTORS_NUM; if (IfIndex>>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; /* for (i=0;i>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; if(MotorControlConfig[index].m_isEnabled ) { int MotorId = ThreadMotorIdToMotorId[index]; _speed = MotorGetSpeedFromFPGA_Res ((TimerMotors_t)MotorId); } return OK; } uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //#define MAX_CONTROL_SAMPLES 6 //extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; //extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM]; //read value is the dancer angle int i,index=MAX_THREAD_MOTORS_NUM; int DancerId; static int pooler_counter = 0; int32_t TranslatedReadValue, avreageSampleValue = 0; uint32_t calculated_speed; double NormalizedError; if (IfIndex>>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); return 0xFFFFFFFF; } index = IfIndex&0xFF; /*for (i=0;i= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0; for (i=0;i DancerStopActivityLimit[index])&&(JobCounter > eOneSecond)) { EndState(CurrentJob); } NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; MotorControlConfig[index].m_mesuredParam = NormalizedError; DancerError[DancerId] = NormalizedError; MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam, &MotorControlConfig[index].m_params, &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral); if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError); else KeepNormalizedError = NormalizedError; calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; if (abs(calculated_speed-CurrentControlledSpeed[index])>5) { CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); } } if (index == FEEDER_MOTOR) { pooler_counter++; if (pooler_counter>=1000) { //float error_integered = MotorControlConfig[index].m_calculatedError*1000; /*{ "HeaterGroupId": 0, "Zone1Temp": 80, "Zone2Temp": 2641, "Heater1Active": false, "Heater2Active": false, "Heater1Percentage": 3, "Heater2Percentage": 4000, "InfoMessage": "Standard DC" } void HeatingTestSendResonse(uint32_t status, bool last,bool heater1Active,bool heater2Active, int temperature1, int temperature2,int Heater1Percentage,int Heater2Percentage, char* Message) }*/ //HeatingTestSendResonse(0, false,true,true, MotorDriverRequest[22].Speed,MotorDriverRequest[18].Speed,MotorDriverRequest[15].Speed,MotorDriverRequest[3].Speed, "MotorSpeed"); HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/_speed,OriginalMotorSpd_2PPS[index]/*(int)error_integered*/,MotorControlConfig[index].m_calculatedError,ReadValue, "FeederSpeed"); pooler_counter = 0; } } return OK; } //******************************************************************************************************************** //******************************************************************************************************************** uint32_t ThreadInitialTestStub(HardwareMotor * request) { //MotorsConfigMessage(request); ThreadPrepareState(request); ThreadPreSegmentState(request); return OK; } bool InitialProcess = false; //******************************************************************************************************************** uint32_t ThreadPrepareState(void *JobDetails) { int Motor_i, HW_Motor_Id, Pid_Id; CurrentSegmentId = 0; JobCounter = 0; //start thread control for all motors for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/ MotorControlConfig[Motor_i].m_params.MAX = 1; MotorControlConfig[Motor_i].m_params.MIN = MotorsControl[Pid_Id].outputproportionalpowerlimit*-1; MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime; MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain; MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime; MotorControlConfig[Motor_i].m_params.epsilon = 0.1; MotorControlConfig[Motor_i].m_params.dt = 50; 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 if (MotorsCfg[HW_Motor_Id].has_directionthreadwize) MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled { if (SpeedControlId != 0xFF) { RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction); SpeedControlId = 0xFF; } //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); SpeedControlId = AddControlCallback(ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); } if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled { if (ControlIdtoMotorId[Motor_i] != 0xFF) { RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction); ControlIdtoMotorId[Motor_i] = 0xFF; CurrentControlledSpeed[Motor_i] = 0; } ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); //AddControlCallback(ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); } if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled { if (ControlIdtoMotorId[Motor_i] != 0xFF) { RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction); CurrentControlledSpeed[Motor_i] = 0; ControlIdtoMotorId[Motor_i] = 0xFF; } ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); } if (Motor_i == WINDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled { if (ControlIdtoMotorId[Motor_i] != 0xFF) { RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction); CurrentControlledSpeed[Motor_i] = 0; ControlIdtoMotorId[Motor_i] = 0xFF; } ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); } /*if (HW_Motor_Id == 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,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0); // continue; AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,ThreadMotorIdToDancerId[Motor_i],Motor_i); else if ((HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)) AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);*/ 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,(IfTypeThread*0x100+Motor_i),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,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); } PrepareReady(Module_Thread,ModuleDone); //set 3 dancers to the profile positions InitialProcess = true; return OK; } void SetOriginMotorSpeed(float process_speed) { int Motor_i, HW_Motor_Id; for (Motor_i = 0; Motor_i <= WINDER_MOTOR; Motor_i++) { HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i]; //(Speed*uStep*PPR)/((2*PI*motor_Radius) // double motor_speed = (process_speed * MotorsCfg[HW_Motor_Id].pulseperround * MotorsCfg[HW_Motor_Id].microstep)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius); double motor_speed = (process_speed * MotorsCfg[HW_Motor_Id].pulseperround) / (2 * PI * MotorsCfg[HW_Motor_Id].pulleyradius); //MotorControlConfig[Motor_i].m_SetParam = motor_speed; OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed; } } //******************************************************************************************************************** uint32_t ThreadPreSegmentState(void *JobDetails) { //set the speed only before the first segment, speed is constant accros job JobTicket* JobTicket = JobDetails; float process_speed = JobTicket->processparameters->dyeingspeed; SetOriginMotorSpeed(process_speed); //ControlStart(); // set the new speed in the dryer motor to the speed of the new segment MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]); //only for testing - when control works, these motors will take their speed from the dryer //MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING, OriginalMotorSpd_2PPS[POOLER_MOTOR]); //only for testing - when control works, these motors will take their speed from the dryer //MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING, OriginalMotorSpd_2PPS[FEEDER_MOTOR]); //#warning rocker disabled if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].has_directionthreadwize) MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize); MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 5); if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].has_directionthreadwize) MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize); MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 5); //#warning rocker disabled // MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 0, GPI_LS_RLOADMOTOR_UP, EndState); //TODO // 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 if ((InitialProcess==false) && JobTicket->enableintersegment == true) { ThreadUpdateProcessLength (JobTicket->intersegmentlength,(void *)ThreadInterSegmentEnded); } else { ThreadUpdateProcessLength (0,(void *)NULL); PreSegmentReady(Module_Thread,ModuleDone); InitialProcess = false; } return OK; } void ThreadInterSegmentEnded(void) { PreSegmentReady(Module_Thread,ModuleDone); } void ThreadSegmentEnded(void) { SegmentReady(Module_Thread,ModuleDone); } void ThreadDistanceToSpoolEnded(void) { } double seglength = 0.0; //******************************************************************************************************************** uint32_t ThreadSegmentState(void *JobDetails, int SegmentId) { JobTicket* JobTicket = JobDetails; seglength = JobTicket->segments[SegmentId]->length; CurrentSegmentId = SegmentId; ThreadUpdateProcessLength (seglength,(void *)ThreadSegmentEnded); return OK; } //******************************************************************************************************************** uint32_t ThreadEndState(void *JobDetails) { int Motor_i; ThreadUpdateProcessLength (0.0,(void *)NULL); SetOriginMotorSpeed(0); if (SpeedControlId != 0xFF) { RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction); SpeedControlId = 0xFF; } for ( Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++) { if (ControlIdtoMotorId[Motor_i] != 0xFF) { RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction); } MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); } MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz); return OK; } //******************************************************************************************************************** void ThreadStartPrinting(void) { //PrintingIterate(); } //******************************************************************************************************************** //******************************************************************************************************************** void ThreadStopPrinting(void) { //PrintingIterate(); }