From 6d0d04a9f1d3ebbc679190ff49df69406eabe24a Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Sun, 26 Jan 2020 16:28:56 +0200 Subject: updating : special dispensers handling (for Moti), new process parameters, bugs and features. merged with Shai --- .../Embedded/Modules/Thread/ThreadLoad.c | 3 + .../Embedded/Modules/Thread/Thread_print.c | 82 +++++++++++++++++----- 2 files changed, 68 insertions(+), 17 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 049e64a64..b9bbf3aab 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c @@ -401,6 +401,8 @@ CallbackCounter++; //MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 200, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000); MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 0, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000); + HeadCard_Actuators_Control(ACTOT, LOW,true); + return OK; } uint32_t Thread_Load_Lift_Dancers(void) @@ -508,6 +510,7 @@ CallbackCounter++; // MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 200, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000); MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 2, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000); + HeadCard_Actuators_Control(ACTOT, LOW,false); return OK; } diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 1c3326336..2484b9444 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -446,6 +446,7 @@ char TMessage[150]; //char time[150]; uint16_t BreakSensorCounter = 0; uint16_t BreakSensorLatchCounter = 0; +bool FirstCalcInJob = true; uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //#define MAX_CONTROL_SAMPLES 6 @@ -453,7 +454,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) //extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM]; //read value is the dancer angle - int i,index=MAX_THREAD_MOTORS_NUM; + int i,index=MAX_THREAD_MOTORS_NUM,len; int DancerId; int32_t TranslatedReadValue, avreageSampleValue = 0;//,avreageMotorSampleValue = 0; //double tempcalcspeed = 0; @@ -604,14 +605,15 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) return OK; } NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; + if ((index != FEEDER_MOTOR)||(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)) //feeder unit handles errors opposite to left unit + { + NormalizedError = (-1*NormalizedError); + } + 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)||(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)) //feeder unit handles errors opposite to left unit - { - MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError); - } /*else { //KeepNormalizedError = NormalizedError; @@ -637,14 +639,37 @@ 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 (FirstCalcInJob == true) + { + if (index == FEEDER_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", + ReadValue,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); + ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,__LINE__,DancerId,RpError,ReadValue,0); + + } + } + #ifndef TEST_PID_THREAD if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue) #else if (index == FEEDER_MOTOR) //feeder unit handles errors opposite to left unit #endif { - CurrentControlledSpeed[index] = calculated_speed; - MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); + if (calculated_speed>5.0) + { + CurrentControlledSpeed[index] = calculated_speed; + MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); + } + else + { + if (calculated_speed<0) + ReportWithPackageFilter(ThreadFilter,"Negative speed",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0); + } + /*if (((JobCounter % 2000) == index*100)&&(index == WINDER_MOTOR)) //feeder unit handles errors opposite to left unit { ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0); @@ -719,7 +744,7 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request) //******************************************************************************************************************** uint32_t ThreadPrepareState(void *JobDetails) { - int Motor_i, HW_Motor_Id, Pid_Id; + int Motor_i,i, HW_Motor_Id, Pid_Id; JobTicket* JobTicket = JobDetails; CurrentSegmentId = 0; @@ -742,6 +767,7 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request) EnableIntersegment = JobTicket->enableintersegment; IntersegmentLength = JobTicket->intersegmentlength; + FirstCalcInJob = true; if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false) { ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING; @@ -791,7 +817,17 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request) 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 - +////////////////////////////////////////////////// + for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++) + { + if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled + MotorSamples[Motor_i][i] = Control_Read_Dancer_Position(ThreadMotorIdToDancerId[Motor_i],0); //reset the samples value for control beginning + else if ((Motor_i == POOLER_MOTOR)||(Motor_i == FEEDER_MOTOR)) + MotorSamples[Motor_i][i] = DancersCfg[ThreadMotorIdToDancerId[Motor_i]].zeropoint; + //MotorSpeedSamples[Motor_i][i] = 0; + } + MotorSamplePointer[Motor_i] = 0; +///////////////////////////////////////////////////// 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 @@ -888,10 +924,11 @@ void SetOriginMotorSpeed(float process_speed) OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed; InitialDryerSpeed = 0.0; CurrentControlledSpeed[Motor_i] = (int) motor_speed; - //ReportWithPackageFilter(ThreadFilter,"Original Speed",__FILE__,Motor_i,motor_speed,RpWarning,process_speed,0); + if (process_speed > 1) + ReportWithPackageFilter(ThreadFilter,"Original Speed",__FILE__,Motor_i,(int)motor_speed,RpWarning,process_speed,0); - for (i = 0; i <= MAX_CONTROL_SAMPLES; i++) - MotorSpeedSamples[Motor_i][i] = motor_speed; + // for (i = 0; i <= MAX_CONTROL_SAMPLES; i++) + // MotorSpeedSamples[Motor_i][i] = motor_speed; } } void ThreadPreSegmentEnded(void) @@ -900,7 +937,7 @@ void ThreadPreSegmentEnded(void) REPORT_MSG (0,"First ThreadPreSegmentEnded"); PreSegmentReady(Module_Thread,ModuleDone); } -int DrierDivider = 10; +int DrierDivider = 20; uint32_t ThreadDryerRampUp(uint32_t IfIndex, uint32_t BusyFlag) { InitialDryerSpeed += (OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider); @@ -943,7 +980,14 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId) PrepareState = false; #ifndef TEST_PID_THREAD // set the new speed in the dryer motor to the speed of the new segment - DrierDivider = dyeingspeed/5; //ramp up drier in 5 cm/sec steps + if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false) + { + DrierDivider = dyeingspeed/3; //ramp up drier in 5 cm/sec steps + } + else + { + DrierDivider = dyeingspeed/5; //ramp up drier in 5 cm/sec steps + } ReportWithPackageFilter(ThreadFilter,"Drier ramp up",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)DrierDivider,0); InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider; MotorSetSpeed(ThreadMotorIdToMotorId[DRYER_MOTOR],InitialDryerSpeed ); @@ -1065,7 +1109,7 @@ char Endstr[150]; { int Motor_i; ThreadControlActive = false; - + uint32_t status = OK,tempCtl; usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength); SendJobProgress(0.0,0,false, Endstr); ReportWithPackageFilter(ThreadFilter,Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); @@ -1091,15 +1135,19 @@ char Endstr[150]; for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) { + tempCtl = ControlIdtoMotorId[Motor_i]; if (ControlIdtoMotorId[Motor_i] != 0xFF) { - if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction) == OK) + status = RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction); + if(status == OK) ControlIdtoMotorId[Motor_i] = 0xFF; else ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)ControlIdtoMotorId[Motor_i],0); } + //ReportWithPackageFilter(ThreadFilter,"Remove Control",__FILE__,Motor_i,(int)status,RpError,(int)tempCtl,0); + } - Task_sleep(10); + Task_sleep(100); for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) { MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); -- cgit v1.3.1