From f952ee0e6e7ca0633d49ad1da9e209a0a3aa4621 Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Wed, 1 Aug 2018 18:03:18 +0300 Subject: handle heater off command, improves screw speed, improve diagnostics --- .../Embedded_SW/Embedded/Modules/Thread/Thread.h | 1 + .../Embedded/Modules/Thread/Thread_Winder.c | 32 ++++++++++++++++++++-- .../Embedded/Modules/Thread/Thread_print.c | 7 +++-- 3 files changed, 34 insertions(+), 6 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 5084114c6..3a0b25ba8 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h @@ -40,6 +40,7 @@ extern HardwareMotor MotorsCfg[NUM_OF_MOTORS]; extern HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM]; //extern InternalWinderConfigStruc InternalWinderCfg; extern HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS]; +extern uint32_t CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM]; #define MAX_CONTROL_SAMPLES 10 extern int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index 0680e97a4..30cbd4db3 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -168,6 +168,8 @@ numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__ uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction) { uint32_t NumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; + float screw_speed = 0; + float RotationsPerSecond; if (Counter) { @@ -176,6 +178,15 @@ uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction) NumberOfSteps -= (Counter/InternalWinderCfg.spoolbackingrate); REPORT_MSG(ScrewNumberOfSteps, "Head Backing"); ScrewNumberOfSteps--; + screw_speed = InternalWinderCfg.segmentoffsetpulses / InternalWinderCfg.NumberOfRotationPerPassage; + // calculation input#2: number of rotations per second - (basically: speed/winder perimeter. later - according to winder actual speed - calculate according to winder position accumulation in the last second. + RotationsPerSecond = dyeingspeed / (InternalWinderCfg.diameter * PI); + RotationsPerSecond = CurrentControlledSpeed[WINDER_MOTOR] / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround; + // calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm. + ScrewSpeed = screw_speed*RotationsPerSecond; + + MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); + } if ((Counter%InternalWinderCfg.SpoolBottomBackingRate == 0)||(Counter%InternalWinderCfg.SpoolBottomBackingRate == 1)) { @@ -193,6 +204,9 @@ uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction) } uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag) { + uint32_t Steps; + + if (BusyfirstCall) { BusyfirstCall = false; @@ -202,6 +216,7 @@ uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag) if (BusyFlag == NOTBUSY) { CalculateNumberOfSteps (DirectionChangeCounter++, ScrewCurrentDirection); + Steps = ScrewNumberOfSteps; if (ScrewCurrentDirection == false) { ScrewCurrentDirection = true; @@ -210,7 +225,18 @@ uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag) { ScrewCurrentDirection = false; } - MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back. + if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT) + { + ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize); + Steps += InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; + REPORT_MSG(LIMIT, "Winder at right limit"); + } + else if (FPGA_Read_limit_Switches(GPI_LS_SCREW_LEFT)==LIMIT) + { + ScrewCurrentDirection = (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize); + REPORT_MSG(LIMIT, "Winder at left limit"); + } + MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,Steps); //process: set point 0, set max speed, move to the specified length, return back. } /* * calculate new ScrewSpeed and call MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); @@ -234,7 +260,6 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId) float screw_speed = 0; float RotationsPerSecond; - int process_speed = dyeingspeed; if (dyeingspeed == 0) { LOG_ERROR (dyeingspeed," job speed zero"); @@ -251,7 +276,8 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId) // * calculation input: traverse length in milimeters/pulses, number of rotations per traverse ==> length of traverse per rotation. screw_speed = InternalWinderCfg.segmentoffsetpulses / InternalWinderCfg.NumberOfRotationPerPassage; // calculation input#2: number of rotations per second - (basically: speed/winder perimeter. later - according to winder actual speed - calculate according to winder position accumulation in the last second. - RotationsPerSecond = process_speed / (InternalWinderCfg.diameter * PI); + RotationsPerSecond = dyeingspeed / (InternalWinderCfg.diameter * PI); + RotationsPerSecond = CurrentControlledSpeed[WINDER_MOTOR] / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround; // calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm. ScrewSpeed = screw_speed*RotationsPerSecond; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index b6c1fea57..75d54ef02 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -360,7 +360,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) if (ReadBreakSensor()==ERROR) { //LOG_ERROR(index, "ReadBreakSensor Error"); - JobEndReason = JOB_DANCER_FAIL; + JobEndReason = JOB_THREAD_BREAK; //SendJobProgress(0.0,0,false, "ReadBreakSensor Error"); EndState(CurrentJob,"ReadBreakSensor Error" ); } @@ -372,7 +372,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { usnprintf(Message, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint); JobAbortedByUser = true; - JobEndReason = JOB_THREAD_BREAK; + JobEndReason = JOB_DANCER_FAIL; EndState(CurrentJob,Message ); } NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; @@ -404,7 +404,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) KeepReadValue = TranslatedReadValue; } }*/ - if (abs(calculated_speed-CurrentControlledSpeed[index])>5) + if (abs(calculated_speed-CurrentControlledSpeed[index])>2) { CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); @@ -533,6 +533,7 @@ void SetOriginMotorSpeed(float process_speed) / (2 * PI * MotorsCfg[HW_Motor_Id].pulleyradius); //MotorControlConfig[Motor_i].m_SetParam = motor_speed; OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed; + CurrentControlledSpeed[Motor_i] = (int) motor_speed; } } -- cgit v1.3.1 From 4bbece442e2699e88a090de1df07e2295d4fdad8 Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Wed, 1 Aug 2018 18:48:10 +0300 Subject: fix heaters off command. fix memory leak --- .../Embedded/Modules/Control/MillisecTask.c | 24 +++++++++++----------- .../Embedded/Modules/Heaters/Heaters_print.c | 10 +++++---- .../Embedded/Modules/Thread/Thread_Winder.c | 3 ++- .../Embedded/Modules/Thread/Thread_print.c | 6 +++--- 4 files changed, 23 insertions(+), 20 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c index 28cdc3702..8243c5faa 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c @@ -226,7 +226,7 @@ int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Leng MSBacklog[MsecLogindex]=Data; Motor_Id[MsecLogindex]=MotorId; MsecLogindex++; - if (MsecLogindex>=999) + if (MsecLogindex>=199) MsecLogindex = 0; //========================== if (MotorId >= NUM_OF_MOTORS) return -1; @@ -247,7 +247,7 @@ int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Len MSBacklog[MsecLogindex]=Data; Motor_Id[MsecLogindex]=MotorId; MsecLogindex++; - if (MsecLogindex>=999) + if (MsecLogindex>=199) MsecLogindex = 0; //========================== MotorActivity++; @@ -389,21 +389,21 @@ uint32_t MillisecLoop(uint32_t tick) } if (Tick98) { - //MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING); - //MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING); - //MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING); - //MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_WINDER); - //MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_SCREW); + MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING); + MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING); + MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING); + MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_WINDER); + MotorGetSpeedFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_SCREW); } if (Hundred_msTick) { for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++) MillisecReadFromTempSensor(Sensor_i, NULL); - //MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING); - //MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING); - //MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING); - //MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_WINDER); - //MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_SCREW); + MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING); + MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING); + MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING); + MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_WINDER); + MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_SCREW); } if (O900Millisecond_Tick) diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c index d8cfdb846..ff033dbda 100644 --- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c @@ -231,17 +231,19 @@ uint32_t HeaterCommandRequestMessage(int HeaterId, bool OnOff, int Temperature) //set the target operation temperature //set the heater operation mode to fast heating - depended on the current temperature } - else if ((HeaterState == HEATER_ON)&& (OnOff == HEATER_OFF)) //stop heating + else if (OnOff == HEATER_OFF)//((HeaterState == HEATER_ON)&& (OnOff == HEATER_OFF)) //stop heating { //turn off the heater HeatersSingleHeaterEnd(HeaterId); //stop the control HeaterPIDConfig[HeaterId].m_SetParam = 0; } - /*else + /*else //heater off, and the heater is not ready yet { - LOG_ERROR (OnOff, "unsupported command id"); - return ERROR; + //turn off the heater + HeatersSingleHeaterEnd(HeaterId); + //stop the control + HeaterPIDConfig[HeaterId].m_SetParam = 0; }*/ return OK; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index 30cbd4db3..794559d57 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -225,7 +225,7 @@ uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag) { ScrewCurrentDirection = false; } - if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT) +/* if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT) { ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize); Steps += InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; @@ -236,6 +236,7 @@ uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag) ScrewCurrentDirection = (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize); REPORT_MSG(LIMIT, "Winder at left limit"); } +*/ MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,Steps); //process: set point 0, set max speed, move to the specified length, return back. } /* diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 75d54ef02..8d1ea20fa 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -127,8 +127,8 @@ void ThreadUpdateProcessLength (double length, void *Funcptr) ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr; initialpos = 0xFFFF; } -double MotorSentData[100] = {0}; -uint32_t PosDif[100] = {0}; +double MotorSentData[102] = {0}; +uint32_t PosDif[102] = {0}; int MotorDataIndex = 0; @@ -181,7 +181,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) //PosDif[MotorDataIndex] = positionDiff; MotorSentData[MotorDataIndex] = length; MotorDataIndex+=1; - if (MotorDataIndex == 99) MotorDataIndex = 0; + if (MotorDataIndex >= 97) MotorDataIndex = 0; static int pooler_counter = 0; pooler_counter++; TotalProcessedLength+= (length/100); -- cgit v1.3.1