From cfde5403e86ce1ace1b5321a31ad0529c0e3ace5 Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Wed, 17 Jun 2020 14:42:54 +0300 Subject: blower alarm and handling, P01 config word, improve tension handling, --- .../Embedded/Modules/Thread/Thread_print.c | 31 +++++++++++++++++----- 1 file changed, 25 insertions(+), 6 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c') diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index efcbd5030..72b0439ad 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -674,9 +674,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) }*/ calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index]; + //if (JobCounter % eHundredMillisecond == 50) if (FirstCalcInJob == true) { - if (index == FEEDER_MOTOR) + if (index == POOLER_MOTOR) { FirstCalcInJob = false; len = usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d", @@ -833,7 +834,7 @@ uint32_t Release_Right_TFU_Tension() { Report("Release_Right_TFU_Tension",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4,0); RTFU_Up = false; - status = MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4, Release_Right_TFU_TensionCallback,1000); + status = MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/2, Release_Right_TFU_TensionCallback,1000); } return status; @@ -859,6 +860,21 @@ uint32_t Adjust_Right_TFU_Tension(double tension) return status; } +int PrepareWaitCount = 0; +uint32_t ThreadPrepare_TensionCallback (int DancerId, double tension) +{ + if (PrepareWaitCount) + { + ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,DancerId,RpWarning,PrepareWaitCount,0); + PrepareWaitCount--; + } + if (PrepareWaitCount == 0) + { + PrepareState = false; + ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback Prepare Ready",__FILE__,__LINE__,DancerId,RpWarning,PrepareWaitCount,0); + PrepareReady(Module_Thread,ModuleDone); + } +} uint32_t ThreadPrepare_Tension (int DancerId, double tension) { int current, request = (int)tension,movement; @@ -892,6 +908,7 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) return status; else { + PrepareWaitCount++; if (current < request) //go down { direction = MotorsCfg[HW_Motor_Id].directionthreadwize; @@ -903,12 +920,12 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) movement = current - request; } MotorSetMaxSpeed (HW_Motor_Id, 500); - MotorMoveWithCallback (HW_Motor_Id, direction, (movement*MotorsCfg[HW_Motor_Id].microstep), NULL,20000); + MotorMoveWithCallback (HW_Motor_Id, direction, (movement*MotorsCfg[HW_Motor_Id].microstep), ThreadPrepare_TensionCallback,20000); status |= MCU_E2PromProgram(address,request); } usnprintf(Lenstr, 100, "ThreadPrepare_Tension Dancer %d Request: %d Current %d movement %d dir %d motor %d address %d", DancerId,request,current,movement,direction,HW_Motor_Id,address); - ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,address,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpFatalError,LIMIT,0); + ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,address,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpWarning,PrepareWaitCount,0); return status; } @@ -1107,7 +1124,8 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) #ifdef TEST_PID_THREAD testDancersControl(); #endif - PrepareReady(Module_Thread,ModuleDone); + if (PrepareWaitCount == 0) + PrepareReady(Module_Thread,ModuleDone); //set 3 dancers to the profile positions return OK; } @@ -1188,7 +1206,8 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId) { SetOriginMotorSpeed(process_speed); ThreadControlActive = true; - PrepareState = false; + if (PrepareWaitCount == 0) + PrepareState = false; PullerSpeedIndex = 0; FeederSpeedIndex = 0; -- cgit v1.3.1 From 5d9f09e5beeb61671ab1a68b021ec6da62bc3042 Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Thu, 18 Jun 2020 17:27:56 +0300 Subject: Version 1.4.6.32; enable variating control timing for thread. fix WHS pid --- .../Embedded_SW/Embedded/Common/SW_Info/SW_Info.c | 2 +- .../WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.c | 22 +++++++++--------- .../Embedded/Modules/General/GeneralHardware.c | 16 ++++++------- .../Embedded/Modules/Thread/Thread_print.c | 27 +++++++++++++++++++++- 4 files changed, 46 insertions(+), 21 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c') diff --git a/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c b/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c index d1602c3cf..c48538752 100644 --- a/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c +++ b/Software/Embedded_SW/Embedded/Common/SW_Info/SW_Info.c @@ -20,7 +20,7 @@ typedef struct } TangoVersion_t; -TangoVersion_t _gTangoVersion = {1,4,6,31}; +TangoVersion_t _gTangoVersion = {1,4,6,32}; #define BUILD_DATE __DATE__ char Dat[50] = BUILD_DATE; char _gTangoName [MAX_STRING_LEN] = "Tango01 ";//d diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.c index be84e9b19..7590113c1 100644 --- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.c +++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.c @@ -42,7 +42,7 @@ typedef struct WhsControlConfig_t WHS_ControlData = {0}; #define MAX_WHS_CONTROL_SAMPLES 10 -int32_t WhsBlowerSamples[MAX_WHS_CONTROL_SAMPLES] ; +float WhsBlowerSamples[MAX_WHS_CONTROL_SAMPLES] ; int WhsBlowerSamplesPointer = 0; bool Read_Max5805_device_ID(); @@ -224,9 +224,9 @@ bool WHS_init_Blower() WHS_ControlData.m_params.Kp = 350; WHS_ControlData.m_params.Ki = 600; WHS_ControlData.m_params.IntegralErrorMultiplier = 100; - WHS_ControlData.m_params.ProportionalErrorMultiplier = 1000; - WHS_ControlData.m_params.epsilon = 0; - WHS_ControlData.m_params.dt = 2; + WHS_ControlData.m_params.ProportionalErrorMultiplier = 100; + WHS_ControlData.m_params.epsilon = 0.2; + WHS_ControlData.m_params.dt = 5; //WHS_ControlData.m_ingnoreValue = PID_Request->sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses WHS_ControlData.m_calculatedError = 0; WHS_ControlData.m_integral = 0; @@ -611,7 +611,7 @@ char whs_str[150]; uint32_t WHS_Pid_Testing_Func(double setParam,double measuredParam) { float calculated_speed; - /*float avreageSampleValue = 0; + float avreageSampleValue = 0; int i; WhsBlowerSamples[WhsBlowerSamplesPointer] = measuredParam;//(-1 * TranslatedReadValue); @@ -622,7 +622,7 @@ uint32_t WHS_Pid_Testing_Func(double setParam,double measuredParam) avreageSampleValue += WhsBlowerSamples[i]; avreageSampleValue = avreageSampleValue / (int)WHS_ControlData.SamplesFilterSize; - WHS_ControlData.m_mesuredParam = avreageSampleValue;*/ + /*WHS_ControlData.m_mesuredParam = avreageSampleValue;*/ WHS_ControlData.m_mesuredParam = measuredParam; WHS_ControlData.m_SetParam = setParam; WHS_ControlData.m_calculatedError = PIDAlgorithmCalculation((float)WHS_ControlData.m_SetParam , (float)WHS_ControlData.m_mesuredParam, @@ -632,10 +632,10 @@ uint32_t WHS_Pid_Testing_Func(double setParam,double measuredParam) calculated_speed = volt + 100; if (calculated_speed < (volt - 100)) calculated_speed = volt - 100;*/ - usnprintf(whs_str, 150, "WHS_Pid_Testing_Func meas %d set %d error %d integral %d blower %d", - (int)(measuredParam*100),(int)(setParam*100),(int)(WHS_ControlData.m_calculatedError*100),(int)(WHS_ControlData.m_integral*100),(int)(calculated_speed)); + usnprintf(whs_str, 150, "WHS_Pid_Testing_Func meas %d avg %d set %d error %d integral %d blower %d", + (int)(measuredParam*100),(int)(avreageSampleValue*100),(int)(setParam*100),(int)(WHS_ControlData.m_calculatedError*100),(int)(WHS_ControlData.m_integral*100),(int)(calculated_speed)); Report(whs_str, __FILE__,__LINE__, (int)(calculated_speed), RpMessage, (int)(volt), 0); - if (fabs(WHS_ControlData.m_calculatedError)>(WHS_ControlData.m_params.MAX/2)) + /*if (fabs(WHS_ControlData.m_calculatedError)>(WHS_ControlData.m_params.MAX/2)) { close_loop_time = 1; WHS_ControlData.m_params.dt = 1; @@ -644,10 +644,10 @@ uint32_t WHS_Pid_Testing_Func(double setParam,double measuredParam) { close_loop_time = 10; WHS_ControlData.m_params.dt = 10; - } + }*/ //close_loop_time = 2; //WHS_ControlData.m_params.dt = 2; - + close_loop_time = WHS_ControlData.m_params.dt; SendLimitedBlowerControl(calculated_speed); return OK; diff --git a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c index 166062248..d2379c309 100644 --- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c +++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c @@ -390,7 +390,7 @@ uint32_t GenHWInitCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag) uint32_t HWConfiguration(UploadHardwareConfigurationRequest* UploadRequest) { uint32_t status = 0; - int Dancer_i, Dispenser_i,PID_i,tempheaterId; + int Dancer_i, Dispenser_i,PID_i,tempheaterId,DispenserId; HardwareBlowerType blowerType = HARDWARE_BLOWER_TYPE__DefaultBlower; if (UploadRequest == NULL) @@ -474,8 +474,7 @@ uint32_t HWConfiguration(UploadHardwareConfigurationRequest* UploadRequest) (request->pidcontrols[PID_i]->hardwarepidcontroltype < HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ7)) status += IDS_DispenserPidRequestMessage(request->pidcontrols[PID_i]); else if (request->pidcontrols[PID_i]->hardwarepidcontroltype == HARDWARE_PID_CONTROL_TYPE__WasteControl) - status += WHS_PidRequestMessage(request->pidcontrols[PID_i]); - + WHS_PidRequestMessage(request->pidcontrols[PID_i]); } } else @@ -488,15 +487,16 @@ uint32_t HWConfiguration(UploadHardwareConfigurationRequest* UploadRequest) { for (Dispenser_i = 0; Dispenser_i < request->n_dispensers ; Dispenser_i++) { - Valve_Set(IDS_Id_to_AirValve[Dispenser_i], Atm_MidTank_OFF ); //Atm_MidTank_OFF/ON - Valve_Set(IDS_Id_to_CartrideValve[Dispenser_i], Atm_MidTank_OFF ); //Atm_MidTank_OFF/ON + DispenserId = request->dispensers[Dispenser_i]->index; + Valve_Set(IDS_Id_to_AirValve[DispenserId], Atm_MidTank_OFF ); //Atm_MidTank_OFF/ON + Valve_Set(IDS_Id_to_CartrideValve[DispenserId], Atm_MidTank_OFF ); //Atm_MidTank_OFF/ON status += DispenserConfigMessage(request->dispensers[Dispenser_i]); - if (Check_Dispenser_Type(request->dispensers[Dispenser_i]->index) == LS_STATUS_ERROR) + if (Check_Dispenser_Type(DispenserId) == LS_STATUS_ERROR) { - LOG_ERROR (Dispenser_i, "Dispenser identification failed"); + LOG_ERROR (DispenserId, "Dispenser identification failed"); //return ERROR; } - status |= Dispenser_EEPROM_Read_Data(Dispenser_i); + status |= Dispenser_EEPROM_Read_Data(DispenserId); } Calculateinit(); } diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 72b0439ad..e7a333cc2 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -58,6 +58,10 @@ uint32_t JobCounter = 0; MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM]; uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM]; +int MotorTiming[MAX_THREAD_MOTORS_NUM]; +int MotorTimer[MAX_THREAD_MOTORS_NUM]; + + uint32_t PreviousPosition = 0, CurrentPosition = 0; double CurrentRequestedLength = 0.0; double CurrentProcessedLength = 0.0; @@ -506,6 +510,18 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) } index = IfIndex&0xFF; + if (MotorTiming[index]>1) + { + MotorTimer[index]++; + if (MotorTimer[index]>=MotorTiming[index]) + { + MotorTimer[index]=0; + } + else + { + return OK; + } + } if(MotorControlConfig[index].m_isEnabled ) { //if (MotorDriverResponse[ThreadMotorIdToMotorId[index]].Busy == true) @@ -874,6 +890,7 @@ uint32_t ThreadPrepare_TensionCallback (int DancerId, double tension) ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback Prepare Ready",__FILE__,__LINE__,DancerId,RpWarning,PrepareWaitCount,0); PrepareReady(Module_Thread,ModuleDone); } + return OK; } uint32_t ThreadPrepare_Tension (int DancerId, double tension) { @@ -936,7 +953,7 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) JobTicket* JobTicket = JobDetails; uint32_t status = OK; CurrentSegmentId = 0; - + float temp_dt = 0; JobCounter = 0; TotalProcessedLength = 0.0; PoolerTotalProcessedLength = 0.0; @@ -1035,6 +1052,14 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension) 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 + + temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001; + MotorTiming[Motor_i] = (int)temp_dt; + if (MotorTiming[Motor_i]) + { + MotorTimer[Motor_i] = MotorTiming[Motor_i]-1; + ReportWithPackageFilter(ThreadFilter,"MotorTiming",__FILE__,Motor_i,MotorTiming[Motor_i],RpWarning,MotorTimer[Motor_i],0); + } ////////////////////////////////////////////////// for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++) { -- cgit v1.3.1