From 1f69782f90dfa3d48e6b081fd2d25f69cf929fbb Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Tue, 30 Jul 2019 10:20:11 +0300 Subject: Version 1.4.2.8 improved loading, cleaning introduced, time handled --- .../Embedded/Modules/Thread/ThreadLoad.c | 41 ++++++++++++++++++---- 1 file changed, 34 insertions(+), 7 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c index d3a9b96ef..efb3f99a9 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c @@ -232,6 +232,16 @@ //Use Notation How Many Rotations In The Drier, Or Check Against Stopper. Move Slowly return OK; } + uint32_t Thread_Load_Jog_Feeder_To_Middle_Point_Callback(uint32_t deviceID, uint32_t BusyFlag) + { + { + Report("Thread_Load_Jog_Feeder_To_Middle_Point_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); + //storeLoadArmParameters(); + LoadStages++; + ThreadLoadStateMachine(LoadStages); + } + return OK; + } uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue) { @@ -396,13 +406,20 @@ { REPORT_MSG(LoadStages, "Thread Load State Machine step"); CallbackCounter++; - MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 100, FEEDER_DANCER ,true, Thread_Load_HomingCallback,2000); + MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 100, FEEDER_DANCER ,true, Thread_Load_Jog_Feeder_To_Middle_Point_Callback,2000); return OK; } uint32_t Thread_Load_Dryer_MovetoEncoderPosition_Callback(uint32_t MotorId, uint32_t ReadValue) { + Report("Thread_Load_Dryer_MovetoEncoderPosition_Callback",__FILE__,__LINE__,LoadStages,RpMessage,ReadValue,0); MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200); + if(ControlId != 0xFF) + { + MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); + RemoveControlCallback(ControlId, ThreadLoadControlCBFunction ); + ControlId = 0xFF; + } if (CallbackCounter) { CallbackCounter--; @@ -418,17 +435,18 @@ else LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0"); + return OK; } uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue) { Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); - if(ControlId != 0xFF) + /*if(ControlId != 0xFF) { MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); RemoveControlCallback(ControlId, ThreadLoadControlCBFunction ); ControlId = 0xFF; - } - //REPORT_MSG(MotorId, "Thread_Load_Dryer_Loading_Callback Motor Id"); + }*/ + REPORT_MSG(MotorId, "Thread_Load_Dryer_Loading_Callback Motor Id"); MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,10000); return OK; } @@ -437,13 +455,14 @@ { REPORT_MSG(LoadStages, "Thread Load State Machine step"); LoadArmInfo.LoadArmRounds = 20; - uint32_t numberOfSteps = 0; + float numberOfSteps = 0; //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40 SetOriginMotorSpeed(19); // OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000; // CurrentControlledSpeed[FEEDER_MOTOR] = 1000; numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; + //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*3.3*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; Report("Thread Load State Machine step.",__FILE__,LoadStages,numberOfSteps,RpMessage,(int)(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius),0); MotorControlConfig[FEEDER_MOTOR].m_params.MAX = 1; MotorControlConfig[FEEDER_MOTOR].m_params.MIN = MotorsControl[FEEDER_MOTOR].outputproportionalpowerlimit*-1; @@ -534,7 +553,7 @@ MotorControlConfig[POOLER_MOTOR].m_params.MIN = MotorsControl[POOLER_MOTOR].outputproportionalpowerlimit*-1; MotorControlConfig[POOLER_MOTOR].m_params.Kd = MotorsControl[POOLER_MOTOR].derivativetime; MotorControlConfig[POOLER_MOTOR].m_params.Kp = MotorsControl[POOLER_MOTOR].proportionalgain; - MotorControlConfig[POOLER_MOTOR].m_params.Ki = MotorsControl[POOLER_MOTOR].integraltime; + MotorControlConfig[POOLER_MOTOR].m_params.Ki = 0;//MotorsControl[POOLER_MOTOR].integraltime; MotorControlConfig[POOLER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[POOLER_MOTOR].setpointramprateorsoftstartramp; MotorControlConfig[POOLER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[POOLER_MOTOR].outputonoffhysteresisvalue; MotorControlConfig[POOLER_MOTOR].m_params.epsilon = MotorsControl[POOLER_MOTOR].epsilon; @@ -556,7 +575,7 @@ MotorControlConfig[WINDER_MOTOR].m_params.MIN = MotorsControl[WINDER_MOTOR].outputproportionalpowerlimit*-1; MotorControlConfig[WINDER_MOTOR].m_params.Kd = MotorsControl[WINDER_MOTOR].derivativetime; MotorControlConfig[WINDER_MOTOR].m_params.Kp = MotorsControl[WINDER_MOTOR].proportionalgain; - MotorControlConfig[WINDER_MOTOR].m_params.Ki = MotorsControl[WINDER_MOTOR].integraltime; + MotorControlConfig[WINDER_MOTOR].m_params.Ki = 0;//MotorsControl[WINDER_MOTOR].integraltime; MotorControlConfig[WINDER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[WINDER_MOTOR].setpointramprateorsoftstartramp; MotorControlConfig[WINDER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[WINDER_MOTOR].outputonoffhysteresisvalue; MotorControlConfig[WINDER_MOTOR].m_params.epsilon = MotorsControl[WINDER_MOTOR].epsilon; @@ -682,7 +701,10 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue) { ThreadLoadStateMachine(ReadValue); } + return OK; } +uint32_t LoadCounter = 0; + uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //read value is the dancer angle @@ -741,6 +763,11 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); } + LoadCounter++; + if ((LoadCounter % 5001) == 0) + { + Report("MotorSpeedUpdated",__FILE__,index,abs(TranslatedReadValue),RpWarning,(int)CurrentControlledSpeed[index],0); + } return OK; } void ThreadLoadRequest(MessageContainer* requestContainer) -- cgit v1.3.1