From 5f6a459aab9a781dc143d8a6eb214408ead6906d Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Sun, 19 May 2019 19:28:56 +0300 Subject: PID - fabs() enabled small integral values. empty waste indication. --- .../Embedded_SW/Embedded/Modules/Thread/Thread.h | 1 + .../Embedded/Modules/Thread/Thread_init.c | 6 +- .../Embedded/Modules/Thread/Thread_print.c | 138 +++++++++++++++------ 3 files changed, 104 insertions(+), 41 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h index 97f3811c7..19201c708 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h @@ -17,6 +17,7 @@ #include "thread_ex.h" +#define NORMAL_COEF_DIVIDER 100 typedef struct { uint32_t startoffsetpulses; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c index 4454565c1..265c751c6 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -106,18 +106,18 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request) #ifdef TEST_LONGER_PID_THREAD MotorsControl[Motor_i].pvinputfilterfactormode = 10; //test longer control #endif - for (i = 0;i < MotorsControl[Motor_i].pvinputfilterfactormode; i++) + for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++) { MotorSamples[Motor_i][i] = 0; //reset the samples value for control beginning MotorSpeedSamples[Motor_i][i] = 0; } NormalizedErrorCoEfficient[Motor_i] = (2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits); - temp=(10*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm); + temp=(NORMAL_COEF_DIVIDER*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm); NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / temp; // uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; temp = 1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits); - temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm*2); + temp = (temp*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm*3/2); DancerStopActivityLimit[Motor_i] = temp/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); 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 50547656a..25485290b 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -137,13 +137,11 @@ void ThreadUpdateProcessLength (double length, void *Funcptr) CurrentRequestedLength = length*100;//Centimetres CurrentProcessedLength = 0; ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr; - initialpos = 0xFFFF; - Poolerinitialpos = 0xFFFF; } char Lenstr[150]; uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) { - uint32_t positionDiff = 0; + uint32_t positionDiff = 0,prevprev; double length = 0.0; int index = MAX_THREAD_MOTORS_NUM; @@ -172,6 +170,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) PreviousPosition = CurrentPosition; initialpos = 0; } + prevprev = PreviousPosition; positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition); //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; PreviousPosition = CurrentPosition; @@ -181,8 +180,16 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; length = (double)(positionDiff)*LengthCalculationMultiplier; - } + if (length > 1000) + { + usnprintf(Lenstr, 100, "length huge: length %d, diff 0x%x, pos 0x%x prev 0x%x",(int)length*100,(int)positionDiff,PreviousPosition,prevprev); + SendJobProgress(0.0,0,false, Lenstr); + Report(Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0); + + } + + } CurrentProcessedLength+=length; static int pooler_counter = 0; @@ -278,11 +285,11 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) index = IfIndex&0xFF; SpeedSamples[MotorSamplePointer[index]] = speed;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; - if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) + if (MotorSamplePointer[index] >= (int)MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0; - for (i=0;i>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); @@ -405,6 +430,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) } return OK; } + KeepReadValue = ReadValue; TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; if (index == POOLER_MOTOR) { @@ -415,15 +441,15 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) //TranslatedReadValue = 0;//test MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; - if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) + if (MotorSamplePointer[index] >= (int)MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0; #ifdef TEST_LONGER_PID_THREAD else // test: handle tension once in pvinputfilterfactormode milliseconds return OK; #endif - for (i=0;i= 5000) + //if (index == WINDER_MOTOR) //feeder unit handles errors opposite to left unit + //{ + // Report("MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,CurrentControlledSpeed[index],0); + //} + /`*if (JobCounter >= 3000) { MotorSpeedSamples[index][MotorSpeedSamplePointer[index]] = CurrentControlledSpeed[index];//(-1 * TranslatedReadValue); MotorSpeedSamplePointer[index]++; @@ -515,28 +546,45 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) avreageMotorSampleValue = avreageMotorSampleValue / MAX_CONTROL_SAMPLES; //Report("MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,avreageMotorSampleValue,0); OriginalMotorSpd_2PPS[index] = avreageMotorSampleValue; - } - } + }*`/ + }*/ calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index]; +#ifndef TEST_PID_THREAD if (abs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue) +#else + if (index == FEEDER_MOTOR) //feeder unit handles errors opposite to left unit +#endif { - /*if (keepdata == true) - { - calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError; - MotorId[controlIndex] = index; - readValue[controlIndex] = ReadValue; - AveragereadValue[controlIndex] = avreageSampleValue; - calculatedspeed[controlIndex] = calculated_speed; - NormError[controlIndex] - = MotorControlConfig[index].m_mesuredParam; - mIntegral[controlIndex] = MotorControlConfig[index].m_integral; - timestamp[controlIndex] = msec_millisecondCounter; - if (controlIndex++>=MAX_THREAD_CONTROL_LOG) - controlIndex = 0; - }*/ CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); + if (((JobCounter % 2000) == index*100)&&(index == WINDER_MOTOR)) //feeder unit handles errors opposite to left unit + { + Report("MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,CurrentControlledSpeed[index],0); + } +#ifdef TEST_PID_THREAD + int len; + if ((JobCounter % 2000) == index*100) + //if (keepdata == true) + { + /*calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError; + MotorId[controlIndex] = index; + readValue[controlIndex] = ReadValue; + AveragereadValue[controlIndex] = avreageSampleValue; + calculatedspeed[controlIndex] = calculated_speed; + NormError[controlIndex] = MotorControlConfig[index].m_mesuredParam; + mIntegral[controlIndex] = MotorControlConfig[index].m_integral; + timestamp[controlIndex] = msec_millisecondCounter;*/ + 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); + Report(TMessage,__FILE__,__LINE__,DancerId,RpError,ReadValue,0); + //Task_sleep(100); + //if (controlIndex++>=MAX_THREAD_CONTROL_LOG) + // controlIndex = 0; + } +#endif } else MotorFailedSample[index]++; @@ -658,8 +706,10 @@ bool InitialProcess = false; ControlIdtoMotorId[Motor_i] = 0xFF; CurrentControlledSpeed[Motor_i] = 0; } +#ifndef TEST_PID_THREAD 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); +#endif } 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 { @@ -670,7 +720,9 @@ bool InitialProcess = false; CurrentControlledSpeed[Motor_i] = 0; ControlIdtoMotorId[Motor_i] = 0xFF; } +#ifndef TEST_PID_THREAD ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); +#endif } 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 { @@ -681,17 +733,23 @@ bool InitialProcess = false; CurrentControlledSpeed[Motor_i] = 0; ControlIdtoMotorId[Motor_i] = 0xFF; } +#ifndef TEST_PID_THREAD ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); +#endif } // 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,TemplateDataReadCBFunction,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0); 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; } - //testDancersControl(); +#ifdef TEST_PID_THREAD + testDancersControl(); +#endif PrepareReady(Module_Thread,ModuleDone); //set 3 dancers to the profile positions InitialProcess = true; + initialpos = 0xFFFF; + Poolerinitialpos = 0xFFFF; return OK; } @@ -709,6 +767,8 @@ void SetOriginMotorSpeed(float process_speed) //MotorControlConfig[Motor_i].m_SetParam = motor_speed; OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed; CurrentControlledSpeed[Motor_i] = (int) motor_speed; + Report("Original Speed",__FILE__,Motor_i,motor_speed,RpWarning,process_speed,0); + for (i = 0; i <= MAX_CONTROL_SAMPLES; i++) MotorSpeedSamples[Motor_i][i] = motor_speed; } @@ -732,8 +792,10 @@ uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId) SetOriginMotorSpeed(process_speed); ThreadControlActive = true; PrepareState = false; +#ifndef TEST_PID_THREAD // 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]); +#endif #ifdef HUNDRED_MICROSECONDS_DANCER_READ MillisecLogInit(); #endif -- cgit v1.3.1