diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules')
6 files changed, 226 insertions, 182 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c index b40bbe4c7..9eba08678 100644 --- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c +++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c @@ -133,7 +133,7 @@ uint32_t LoadArmRounds; uint32_t D_numberOfSteps = 0; float D_numberOfCycles = 0; -double D_DrierPrevLocation = 0; +uint32_t D_DrierPrevLocation = 0; uint32_t MotorHomingRequestFunc(MessageContainer* requestContainer) @@ -465,30 +465,33 @@ return OK; /******************************************************************************** * Drier Loading Arm Homing ********************************************************************************/ -uint32_t Drier_Center = 0; -float Calculate_Arm_Angle(uint32_t Current_Angle) -{ - uint32_t angle; - float Calc_angle; - if (Current_Angle >= Drier_Center) - angle = Current_Angle - Drier_Center; - else - angle = Current_Angle + 0x3FFF - Drier_Center; - Calc_angle = angle/0x3FFF; - MCU_E2PromProgram(EEPROM_DRIER_LOADING_ARM_ANGLE,Calc_angle); - ReportWithPackageFilter(DiagnosticsFilter,"Calculate_Arm_Angle",__FILE__,(int)(Calc_angle*1000),Current_Angle,RpMessage,Drier_Center,0); - return Calc_angle; -} +uint32_t Drier_Center_read = 0; uint32_t Diagnostics_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag) { MessageContainer responseContainer; MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT; + uint32_t angle, temp = Read_Dryer_ENC_Position(); + angle = Calculate_Arm_Distance(D_DrierPrevLocation,temp); + + if ((angle<14000 )||(ReadValue == BUSY)) // OK - take another round + { + responseContainer.has_error = true; + responseContainer.error = ERROR_CODE__GENERAL_ERROR; + + } ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Set_Load_Arm_To_Stopper time",__FILE__,__LINE__,msec_millisecondCounter,RpMessage,0,0); + if (HomingControlId[deviceID] != 0xff) + { + ReportWithPackageFilter(DiagnosticsFilter,"MotorHomingProgressReport stopped",__FILE__,__LINE__,deviceID,RpMessage,HomingCounter[deviceID],0); + RemoveControlCallback(HomingControlId[deviceID],MotorHomingProgressReport); + HomingControlId[deviceID] = 0xff; + } //NumberOfDrierLoaderCycles=0; //storeLoadArmParameters(); - SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); //set this point as the spool home + //SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); //set this point as the spool home + ThreadLoadingRestartReport(); responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse, HomingToken[deviceID], true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size); responseContainer.has_continuous = true; responseContainer.continuous = true; @@ -501,45 +504,50 @@ uint32_t Diagnostics_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_ return OK; } -uint32_t Diagnostics_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue) +/*uint32_t Diagnostics_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue) { + bool direction; + D_numberOfCycles++; uint32_t temp = Read_Dryer_ENC_Position(); ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_UnLoading_Callback",__FILE__,ReadValue,temp,RpMessage,D_DrierPrevLocation,0); //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_UnLoading_Callback details",__FILE__,(int)(TotalLoadedLen),D_numberOfCycles,RpMessage,CallbackCounter,0); - if ((AccumulatedArmMovement>8000 )&&(ReadValue == NOTBUSY)) // OK - take another round -// if (ReadValue == NOTBUSY) // OK - take another round + //if ((AccumulatedArmMovement>8000 )&&(ReadValue == NOTBUSY)) // OK - take another round + if (ReadValue == NOTBUSY) // OK - take another round { - D_DrierPrevLocation = temp; ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_UnLoading cycles",__FILE__,D_numberOfCycles,LoadArmRounds,RpMessage,0,0); - if (D_numberOfCycles<LoadArmRounds) + MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0); + ReportWithPackageFilter(DiagnosticsFilter,"Store Number of cycles in drier",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); + if (abs(temp -Drier_Center_read)<200) { - MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-D_numberOfCycles); - MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, - D_numberOfSteps, Diagnostics_Dryer_UnLoading_Callback, 10000); - } - else //done enough cycles, go to the center point - { - MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Diagnostics_Set_Load_Arm_To_Stopper_Callback,15000,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize); - MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-D_numberOfCycles); - ReportWithPackageFilter(DiagnosticsFilter,"Store Number of cycles in drier",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); + ReportWithPackageFilter(DiagnosticsFilter,"drier center proximity",__FILE__,temp,Drier_Center_read,RpMessage,abs(temp -Drier_Center_read),0); + if (temp<Drier_Center_read) + direction = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize; + else + direction = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize; + MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Diagnostics_Set_Load_Arm_To_Stopper_Callback,3000,direction,10); + } + else + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop); + } else //timeout or no movement { ReportWithPackageFilter(DiagnosticsFilter,"Unloading drier - halted",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); - MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-(D_numberOfCycles-2));//it takes two cycles to identify a stop of the arm + //MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-(D_numberOfCycles-2));//it takes two cycles to identify a stop of the arm ReportWithPackageFilter(DiagnosticsFilter,"Drier unloading timeout(1) or no movement",__FILE__,temp,D_DrierPrevLocation,RpWarning,ReadValue,0); + Diagnostics_Set_Load_Arm_To_Stopper_Callback(MotorId, ReadValue); } return OK; -} +}*/ uint32_t Diagnostics_Dryer_UnLoading(void) { - uint32_t temp; + D_DrierPrevLocation = Read_Dryer_ENC_Position(); - D_numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; - D_numberOfCycles = 0; + LoadingArmReset(Diagnostics_Set_Load_Arm_To_Stopper_Callback,300000); + /*uint32_t temp; MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds); if (LoadArmRounds == 0) //prev trial stopped @@ -549,16 +557,21 @@ uint32_t Diagnostics_Dryer_UnLoading(void) if (LoadArmRounds <= 2) MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds); + D_numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; + D_numberOfCycles = 0; + MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp); - Drier_Center = temp; + Drier_Center_read = temp; D_DrierPrevLocation = temp; ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,D_DrierPrevLocation,RpMessage, LoadArmRounds,0); MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius); + // status |= MotorMoveToStopper(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize), // MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4, Diagnostics_Set_Load_Arm_To_Stopper_Callback,0,1000); - MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, - D_numberOfSteps-800, Diagnostics_Dryer_UnLoading_Callback, 10000); - + //MotorRunWithCallback + MotorRunWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, + MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius, Diagnostics_Dryer_UnLoading_Callback, 300000); +*/ return OK; } uint32_t Diagnostics_Dryer_MovetoEncoderPosition_Callback(uint32_t MotorId, uint32_t ReadValue) @@ -566,7 +579,7 @@ uint32_t Diagnostics_Dryer_MovetoEncoderPosition_Callback(uint32_t MotorId, uint MessageContainer responseContainer; MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT; - ReportWithPackageFilter(DiagnosticsFilter,"Store angle of arm drier - ended",__FILE__,__LINE__,(int)(Calculate_Arm_Angle(ReadValue)*360),RpMessage,D_numberOfCycles,0); + ReportWithPackageFilter(DiagnosticsFilter,"Store angle of arm drier - ended",__FILE__,__LINE__,(int)(Calculate_Arm_Angle(Drier_Center_read,ReadValue)*360),RpMessage,D_numberOfCycles,0); //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_MovetoEncoderPosition_Callback",__FILE__,__LINE__,0,RpMessage,CallbackCounter,0); MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200); responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse, HomingToken[MotorId], true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size); @@ -584,12 +597,15 @@ uint32_t Diagnostics_Dryer_MovetoEncoderPosition_Callback(uint32_t MotorId, uint uint32_t Diagnostics_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue) { D_numberOfCycles++; + uint32_t angle, temp = Read_Dryer_ENC_Position(); - ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading_Callback",__FILE__,(int)D_numberOfCycles,(int)D_DrierPrevLocation,RpMessage,ReadValue,0); + angle = Calculate_Arm_Distance(D_DrierPrevLocation,temp); + ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading_Callback",__FILE__,(int)angle,(int)ReadValue,RpMessage,temp,0); MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,D_numberOfCycles); - if (ReadValue == NOTBUSY) + if ((angle>14000 )&&(ReadValue == NOTBUSY)) // OK - take another round { + D_DrierPrevLocation = temp; //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading_Callback details",__FILE__,(int)(TotalLoadedLen),numberOfCycles,RpMessage,CallbackCounter,0); if (D_numberOfCycles<LoadArmRounds) @@ -599,14 +615,14 @@ uint32_t Diagnostics_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue } else { - MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Diagnostics_Dryer_MovetoEncoderPosition_Callback,15000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize); + MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Diagnostics_Dryer_MovetoEncoderPosition_Callback,15000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,40); ReportWithPackageFilter(DiagnosticsFilter,"Store Number of cycles in drier",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); } } else { ReportWithPackageFilter(DiagnosticsFilter,"Loading drier - halted",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); - ReportWithPackageFilter(DiagnosticsFilter,"Store angle of arm drier - halted",__FILE__,__LINE__,(int)(Calculate_Arm_Angle(ReadValue)*360),RpMessage,D_numberOfCycles,0); + ReportWithPackageFilter(DiagnosticsFilter,"Store angle of arm drier - halted",__FILE__,__LINE__,(int)(Calculate_Arm_Angle(Drier_Center_read,ReadValue)*360),RpMessage,D_numberOfCycles,0); MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200); } return OK; @@ -620,17 +636,17 @@ uint32_t Diagnostics_Dryer_Loading(void) LoadArmRounds = (int)dryerbufferlength; MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&D_DrierPrevLocation); //MCU_E2PromProgram(EEPROM_DRIER_LOADING_ARM_ANGLE,Calc_angle); - //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading request: current ",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0); - Drier_Center = D_DrierPrevLocation; + ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading request: current ",__FILE__,D_DrierPrevLocation,D_numberOfCycles,RpMessage,LoadArmRounds,0); + Drier_Center_read = D_DrierPrevLocation; D_numberOfCycles = 0; D_numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; - + D_numberOfSteps -= 100; MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius); //6 seconds per round //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, - D_numberOfSteps-800, Diagnostics_Dryer_Loading_Callback, 10000); + D_numberOfSteps, Diagnostics_Dryer_Loading_Callback, 10000); return OK; } diff --git a/Software/Embedded_SW/Embedded/Modules/General/buttons.c b/Software/Embedded_SW/Embedded/Modules/General/buttons.c index 328ff3203..569071c3a 100644 --- a/Software/Embedded_SW/Embedded/Modules/General/buttons.c +++ b/Software/Embedded_SW/Embedded/Modules/General/buttons.c @@ -402,7 +402,10 @@ uint32_t ButtonLoadCBFunction(uint32_t IfIndex, uint32_t ReadValue) case LONGPB: ReportWithPackageFilter(GeneralFilter,"------------ thread loading LONGPB -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0); if (ThreadLoadingActive() == false) - ThreadLoadButton(0); //INIT + { + ReportWithPackageFilter(GeneralFilter,"------------ NOT starting loading from button -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0); + ThreadLoadButton(0); //INIT CHANGE HERE FOR 1.4.6.44 + } else Thread_Load_End(); //END break; diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c index 3c7248204..a6b7bff86 100644 --- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c @@ -218,13 +218,12 @@ void initializeArrays(void) HeaterId2PT100Id[HEATER_TYPE__HeaterZone1] = HEAD_PT100_ZONE_1_0X80_0; HeaterId2PT100Id[HEATER_TYPE__HeaterZone2] = HEAD_PT100_ZONE_2_0X80_1; HeaterId2PT100Id[HEATER_TYPE__HeaterZone3] = HEAD_PT100_ZONE_3_0X82_0; - /* - HeaterId2PT100Id[HEATER_TYPE__HeaterZone4] = HEAD_PT100_ZONE_4_0X82_1; + //HeaterId2PT100Id[HEATER_TYPE__HeaterZone4] = HEAD_PT100_ZONE_4_0X82_1; HeaterId2PT100Id[HEATER_TYPE__HeaterZone5] = HEAD_PT100_ZONE_5_0X84_0; - HeaterId2PT100Id[HEATER_TYPE__HeaterZone6] = HEAD_PT100_ZONE_6_0X84_1;*/ + //HeaterId2PT100Id[HEATER_TYPE__HeaterZone6] = HEAD_PT100_ZONE_6_0X84_1;*/ HeaterId2PT100Id[HEATER_TYPE__MixerHeater] = HEAD_PT100_MIXER_0X8E_0; - /*HeaterId2PT100Id[HEATER_TYPE__HeaterZone7] = HEAD_PT100_ZONE_7_0X86_0; - HeaterId2PT100Id[HEATER_TYPE__HeaterZone8] = HEAD_PT100_ZONE_8_0X86_1; + HeaterId2PT100Id[HEATER_TYPE__HeaterZone7] = HEAD_PT100_ZONE_7_0X86_0; + /*HeaterId2PT100Id[HEATER_TYPE__HeaterZone8] = HEAD_PT100_ZONE_8_0X86_1; HeaterId2PT100Id[HEATER_TYPE__HeaterZone9] = HEAD_PT100_ZONE_9_0X88_0; HeaterId2PT100Id[HEATER_TYPE__HeaterZone10] = HEAD_PT100_ZONE_10_0X88_1; HeaterId2PT100Id[HEATER_TYPE__HeaterZone11] = HEAD_PT100_ZONE_11_0X8A_0; @@ -239,13 +238,12 @@ void initializeArrays(void) HeaterId2CurrentId[HEATER_TYPE__HeaterZone1] = HEAD_CURRENT_ZONE_1; HeaterId2CurrentId[HEATER_TYPE__HeaterZone2] = HEAD_CURRENT_ZONE_2; HeaterId2CurrentId[HEATER_TYPE__HeaterZone3] = HEAD_CURRENT_ZONE_3; - /* - HeaterId2CurrentId[HEATER_TYPE__HeaterZone4] = HEAD_CURRENT_ZONE_4; + //HeaterId2CurrentId[HEATER_TYPE__HeaterZone4] = HEAD_CURRENT_ZONE_4; HeaterId2CurrentId[HEATER_TYPE__HeaterZone5] = HEAD_CURRENT_ZONE_5; - HeaterId2CurrentId[HEATER_TYPE__HeaterZone6] = HEAD_CURRENT_ZONE_6;*/ + //HeaterId2CurrentId[HEATER_TYPE__HeaterZone6] = HEAD_CURRENT_ZONE_6; HeaterId2CurrentId[HEATER_TYPE__MixerHeater] = HEAD_CURRENT_MIXER; - /*HeaterId2CurrentId[HEATER_TYPE__HeaterZone7] = HEAD_CURRENT_ZONE_7; - HeaterId2CurrentId[HEATER_TYPE__HeaterZone8] = HEAD_CURRENT_ZONE_8; + HeaterId2CurrentId[HEATER_TYPE__HeaterZone7] = HEAD_CURRENT_ZONE_7; + /*HeaterId2CurrentId[HEATER_TYPE__HeaterZone8] = HEAD_CURRENT_ZONE_8; HeaterId2CurrentId[HEATER_TYPE__HeaterZone9] = HEAD_CURRENT_ZONE_9; HeaterId2CurrentId[HEATER_TYPE__HeaterZone10] = HEAD_CURRENT_ZONE_10; HeaterId2CurrentId[HEATER_TYPE__HeaterZone11] = HEAD_CURRENT_ZONE_11; diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c index 8856d873a..5da52f590 100644 --- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c +++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c @@ -66,6 +66,7 @@ extern uint16_t Head_Fan_Tach[2]; extern bool doorState; bool RestartNeeded = false; +uint32_t temp1,temp2; void Stub_ProgressRequest(MessageContainer* requestContainer) { @@ -1105,6 +1106,42 @@ void Stub_ProgressRequest(MessageContainer* requestContainer) response.has_progress = true; } else + if(request->amount == 0xB11) //fast refresh for pressure + { + temp1 = Read_Dryer_ENC_Position(); + temp2 = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius/4; + + MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Hiz); + MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,200); + //6 seconds per round + Report("Arm movement test",__FILE__,request->delay,request->delay/2*2,RpWarning,(int)request->delay%2,0); + + //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); + MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, request->delay%2,request->delay/2*2, NULL, 12000); + + response.progress = temp2; + response.has_progress = true; + } + else + if(request->amount == 0xB12) //fast refresh for pressure + { + + temp2 = Calculate_Arm_Distance(temp1,Read_Dryer_ENC_Position()); + Report("Arm movement test calc",__FILE__,temp2,temp1,RpWarning,Read_Dryer_ENC_Position(),0); + + response.progress = temp2; + response.has_progress = true; + } + else + if(request->amount == 0xB13) //TryThreadLoadingRequest + { + TryThreadLoadingFunc(NULL); + Report("TryThreadLoadingFunc",__FILE__,temp2,temp1,RpWarning,Read_Dryer_ENC_Position(),0); + + response.progress = 0xB13; + response.has_progress = true; + } + else if(request->amount == 0xC3) //suspend I2C task { if (request->delay == 0) diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c index cd7c146fe..d16683967 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c @@ -126,6 +126,7 @@ uint32_t Thread_Load_Dryer_UnLoading(void); uint32_t ThreadLoadingReport(void); + uint32_t ThreadLoadingRestartReport(void); //sending after a failure in the finalizing stage bool ThreadLoaded(void); bool ThreadLoadingActive(void) { @@ -245,38 +246,41 @@ } uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag) { + uint32_t angle, temp = Read_Dryer_ENC_Position(); - /*if(PullerControlId != 0xFF) - { - MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz); - RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction ); - PullerControlId = 0xFF; - }*/ - if(WinderControlId != 0xFF) + ReportWithPackageFilter(DiagnosticsFilter,"Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,(int)numberOfCycles,(int)DrierPrevLocation,RpMessage,temp,0); + + angle = Calculate_Arm_Distance(DrierPrevLocation,temp); + + DrierPrevLocation = temp; + if (CallbackCounter) { - MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz); - RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction ); - WinderControlId = 0xFF; + CallbackCounter--; } + if ((angle>14000 )&&(BusyFlag == NOTBUSY)) // OK - take another round + { + Report("Thread_Load_Set_Load_Arm_To_Stopper time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0); - MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW); - /*Task_sleep(5) - MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); - MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/ + Report("Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); + NumberOfDrierLoaderCycles=0; + //storeLoadArmParameters(); + LoadStages++; - MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz); + ThreadLoadStateMachine(LoadStages); + } + else + { + load.color = fastBILNK; + usnprintf(LoadErrorMsg, 100, "Stage %s - %s timeout",LoadStagesStr[LoadStages], MotorStr[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM]); + Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0); + + LoadStatus = ERROR; + ThreadLoadingReport(); + TryAgain = true; + //ThreadLoadButton(LoadStages); + } //move to exact location? // Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); - CallbackCounter = 0; - Report("Thread_Load_Set_Load_Arm_To_Stopper time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0); - - Report("Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); - NumberOfDrierLoaderCycles=0; - //storeLoadArmParameters(); - LoadStages++; - SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); //set this point as the spool home - - ThreadLoadStateMachine(LoadStages); return OK; } uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue) @@ -484,6 +488,8 @@ CallbackCounter++; MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,10000); + Report("Thread_Load_Close dancers tension",__FILE__,__LINE__,(int)windertension,RpMessage,(int)pullertension,0); + // MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000); status |= MCU_E2PromRead(EEPROM_WINDER_TENSION_POSITION,¤t); @@ -666,7 +672,7 @@ } else { - MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,30000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize); + MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,30000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,40); Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); } } @@ -826,6 +832,7 @@ ThreadLoadControlId = 0xFF; } + LoadingStopArmReset(); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_RLOADING); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_LLOADING); @@ -860,7 +867,7 @@ return OK; } - uint32_t Thread_Load_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue) + /*uint32_t Thread_Load_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue) { bool direction; numberOfCycles++; @@ -884,7 +891,7 @@ } else //done enough cycles, go to the center point { - MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,30000,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize); + MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,30000,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,40); MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0); Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); } @@ -902,7 +909,7 @@ direction = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize; else direction = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize; - MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback,3000,direction); + MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_OnError_Callback,3000,direction,10); } else @@ -911,12 +918,6 @@ TimeoutsCounter = 0; CallbackCounter = 0; - /*if(PullerControlId != 0xFF) - { - MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz); - RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction ); - PullerControlId = 0xFF; - }*/ if(WinderControlId != 0xFF) { MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz); @@ -925,9 +926,6 @@ } MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW); - /*Task_sleep(5) - MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); - MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/ MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz); load.color = fastBILNK; @@ -940,79 +938,24 @@ //ThreadLoadButton(LoadStages); } return OK; - } + }*/ uint32_t Thread_Load_Dryer_UnLoading(void) { uint32_t temp; REPORT_MSG(LoadStages, "Thread Load State Machine step Dryer Unloading"); //LoadArmRounds = 0; //uint32_t numberOfSteps = 0; - //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40 - SetOriginMotorSpeed(22); -// OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000; -// CurrentControlledSpeed[FEEDER_MOTOR] = 1000; - -// Rockers are up already, so puller handling is not needed - //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; - /*MotorControlConfig[POOLER_MOTOR].m_params.MAX = 1; - 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 = 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; - MotorControlConfig[POOLER_MOTOR].m_params.dt = MotorsControl[POOLER_MOTOR].controloutputtype; - MotorControlConfig[POOLER_MOTOR].m_ingnoreValue = MotorsControl[POOLER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses - MotorControlConfig[POOLER_MOTOR].m_calculatedError = 0; - MotorControlConfig[POOLER_MOTOR].m_integral = 0; - MotorControlConfig[POOLER_MOTOR].m_isEnabled = true; - MotorControlConfig[POOLER_MOTOR].m_isReady = true; - MotorControlConfig[POOLER_MOTOR].m_mesuredParam = 0; - MotorControlConfig[POOLER_MOTOR].m_preError = 0; - MotorControlConfig[POOLER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage - MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDRIVING].directionthreadwize); - - PullerControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+POOLER_MOTOR),POOLER_DANCER,POOLER_MOTOR); - Report("AddControlCallback Puller",__FILE__,__LINE__,PullerControlId,RpMessage,IfTypeThread*0x100+POOLER_MOTOR,0);*/ -//////////////////////// - MotorControlConfig[WINDER_MOTOR].m_params.MAX = 1; - 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 = 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; - MotorControlConfig[WINDER_MOTOR].m_params.dt = MotorsControl[WINDER_MOTOR].controloutputtype; - MotorControlConfig[WINDER_MOTOR].m_ingnoreValue = MotorsControl[WINDER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses - MotorControlConfig[WINDER_MOTOR].m_calculatedError = 0; - MotorControlConfig[WINDER_MOTOR].m_integral = 0; - MotorControlConfig[WINDER_MOTOR].m_isEnabled = true; - MotorControlConfig[WINDER_MOTOR].m_isReady = true; - MotorControlConfig[WINDER_MOTOR].m_mesuredParam = 0; - MotorControlConfig[WINDER_MOTOR].m_preError = 0; - MotorControlConfig[WINDER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage - MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_WINDER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].directionthreadwize); - - WinderControlId = AddControlCallback(NULL,ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+WINDER_MOTOR),WINDER_DANCER,WINDER_MOTOR); - Report("AddControlCallback Winder",__FILE__,__LINE__,WinderControlId,RpMessage,IfTypeThread*0x100+WINDER_MOTOR,0); - -//////////////////////// - MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING].directionthreadwize); - MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]); CallbackCounter++; - //MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, - // numberOfSteps, Thread_Load_Dryer_Loading_Callback, 100000); - //Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0); - //MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles); -//shlomo - UnloadingStart = msec_millisecondCounter; - numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; - numberOfCycles = 0; - if (SecondTry == true) + MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp); + DrierCenterLocation = DrierPrevLocation; + DrierPrevLocation = temp; + UnloadingStart = msec_millisecondCounter; + //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; + //numberOfCycles = 0; + LoadingArmReset(Thread_Load_Set_Load_Arm_To_Stopper_Callback,300000); + /*if (SecondTry == true) { MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds); if (LoadArmRounds == 0) //prev trial stopped @@ -1028,8 +971,8 @@ { LoadArmRounds = (int)dryerbufferlength; } - } - MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp); + }*/ + /*MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp); DrierCenterLocation = DrierPrevLocation; DrierPrevLocation = temp; Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,UnloadingStart,DrierPrevLocation,RpMessage, LoadArmRounds,0); @@ -1046,6 +989,8 @@ //Keep Notation How Many Rotations In The Dryer //LoadArmRounds = (int)dryerbufferlength; + * + */ load.color = BLINK; return OK; } @@ -1172,6 +1117,8 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue) LoadStatus = OK; SecondTry = true; load.color = BLINK; + if (LoadStages > THREAD_LOAD_INITIAL_TENSION) + ThreadLoadingRestartReport(); Report("Calling State machine retry",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0); ThreadLoadStateMachine(LoadStages); } @@ -1180,6 +1127,8 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue) SecondTry = false; Report("Calling State machine 2nd try",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0); load.color = BLINK; + if (LoadStages > THREAD_LOAD_INITIAL_TENSION) + ThreadLoadingRestartReport(); ThreadLoadStateMachine(LoadStages+1); } else //((SecondTry == false)&&(TryAgain == false))??????? @@ -1191,6 +1140,8 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue) else { Report("Calling State machine status OK",__FILE__,ReadValue,LoadStatus,RpMessage,SecondTry,0); + if (LoadStages > THREAD_LOAD_INITIAL_TENSION) + ThreadLoadingRestartReport(); ThreadLoadStateMachine(LoadStages); SecondTry = false; } @@ -1521,22 +1472,53 @@ uint32_t ThreadLoadingReport(void) return OK; } +bool ThreadLoadingRestartFlag = false; +uint32_t ThreadLoadingRestartReport(void) //sending after a failure in the finalizing stage +{ + MessageContainer responseContainer; + StartThreadLoadingResponse response = START_THREAD_LOADING_RESPONSE__INIT; + + if (ThreadLoadingToken[0] == 0) + return OK; + ThreadLoadingRestartFlag = true; + + response.has_state = true; + response.state = THREAD_LOADING_STATE__ReadyForLoading; + response.errorreason = DefaultErrSrt; + Report("ThreadLoadingReport",__FILE__,MessageState,response.state,RpWarning,(int)LoadStages,0); + //------------------------------------------------------------------------------------------- + responseContainer = createContainer(MESSAGE_TYPE__StartThreadLoadingResponse, ThreadLoadingToken, false, &response, &start_thread_loading_response__pack, &start_thread_loading_response__get_packed_size); + responseContainer.has_continuous = true; + responseContainer.continuous = true; + uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); + size_t container_size = message_container__pack(&responseContainer, container_buffer); + my_free(responseContainer.data.data); + SendChars((char*)container_buffer, container_size); + + return OK; + +} uint32_t TryThreadLoadingFunc(MessageContainer* requestContainer) { - TryThreadLoadingRequest *request = try_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); + TryThreadLoadingRequest *request; TryThreadLoadingResponse Cresponse = TRY_THREAD_LOADING_RESPONSE__INIT; MessageContainer responseContainer; - MessageState = 2; + //MessageState = 2; + if (requestContainer) + request = try_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); ThreadLoadingReport(); Report("TryThreadLoadingFunc",__FILE__,__LINE__,MESSAGE_TYPE__TryThreadLoadingResponse,RpWarning,(int)LoadStages,0); - responseContainer = createContainer(MESSAGE_TYPE__TryThreadLoadingResponse, requestContainer->token, true, &Cresponse, &try_thread_loading_response__pack, &try_thread_loading_response__get_packed_size); - responseContainer.continuous = false; - uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); - size_t container_size = message_container__pack(&responseContainer, container_buffer); - my_free(responseContainer.data.data); - SendChars((char*)container_buffer, container_size); + if (requestContainer) + { + responseContainer = createContainer(MESSAGE_TYPE__TryThreadLoadingResponse, requestContainer->token, true, &Cresponse, &try_thread_loading_response__pack, &try_thread_loading_response__get_packed_size); + responseContainer.continuous = false; + uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); + size_t container_size = message_container__pack(&responseContainer, container_buffer); + my_free(responseContainer.data.data); + SendChars((char*)container_buffer, container_size); + } ThreadLoadButton(LoadStages); return OK; @@ -1544,7 +1526,8 @@ uint32_t TryThreadLoadingFunc(MessageContainer* requestContainer) uint32_t ThreadUpdateCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) { - ThreadLoadingReport(); + if(ThreadLoadingActive()) + ThreadLoadingReport(); return OK; } @@ -1567,13 +1550,16 @@ uint32_t ContinueThreadLoadingFunc(MessageContainer* requestContainer) MessageContainer responseContainer; MessageState = 2; ThreadLoadingReport(); - if (request->processparameters) + if (ThreadLoadingRestartFlag == false) { - dryerbufferlength = request->processparameters->dryerbufferlength; - LoadArmRounds = (int)(request->processparameters->dryerbufferlength); + if (request->processparameters) + { + dryerbufferlength = request->processparameters->dryerbufferlength; + LoadArmRounds = (int)(request->processparameters->dryerbufferlength); + } + Report("ContinueThreadLoadingFunc",__FILE__,__LINE__,(int)(request->processparameters->dryerbufferlength),RpWarning,(int)LoadStages,0); } - Report("ContinueThreadLoadingFunc",__FILE__,__LINE__,(int)(request->processparameters->dryerbufferlength),RpWarning,(int)LoadStages,0); responseContainer = createContainer(MESSAGE_TYPE__ContinueThreadLoadingResponse, requestContainer->token, true, &Cresponse, &continue_thread_loading_response__pack, &continue_thread_loading_response__get_packed_size); responseContainer.continuous = false; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 275e7e5b7..d7bfb1b6e 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -162,7 +162,7 @@ int FeederSpeedIndex = 0,Speed_i; void ThreadUpdateProcessLength (double length, void *Funcptr) { - REPORT_MSG(length,"ThreadUpdateProcessLength"); + ReportWithPackageFilter(ThreadFilter,"ThreadUpdateProcessLength.",__FILE__,__LINE__,(int)length,RpMessage,(int)dyeingspeed,0); CurrentRequestedLength = length*100;//Centimetres CurrentProcessedLength = 0; ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr; @@ -337,7 +337,10 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) }**/ //} - +#ifdef FOUR_WINDERS + if (CurrentControlledSpeed[WINDER_MOTOR]>100) + length = dyeingspeed/10; +#endif PoolerTotalProcessedLength+= (length/100); TempPoolerTotalProcessedLength = PoolerTotalProcessedLength; #ifndef FEEDER_LENGTH_CALCULATION @@ -642,11 +645,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { c++; } - if ((index == WINDER_2_MOTOR)||(index == WINDER_3_MOTOR)) + //if ((index == WINDER_2_MOTOR)||(index == WINDER_3_MOTOR)||(index == WINDER_4_MOTOR)) + /*if (index >= WINDER_MOTOR) { //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); - } + }*/ #endif if (index == POOLER_MOTOR) { @@ -738,12 +742,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index]; //if (0)//(JobCounter % 1000 == 0) #ifdef FOUR_WINDERS - if (JobCounter % 500 < 4)//(FirstCalcInJob == true) + if (0)//(JobCounter % 500 < 7)//(FirstCalcInJob == true) { if (index >= WINDER_MOTOR) { // FirstCalcInJob = false; - len = usnprintf(ATMessage[index], 150, "index %d read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d",index, + len = usnprintf(ATMessage[index], 150, "index %d read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d",index-WINDER_MOTOR+1, TranslatedReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000), (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000), (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed, (int)(InitialDryerSpeed*100/OriginalMotorSpd_2PPS[DRYER_MOTOR])); |