diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c | 281 |
1 files changed, 165 insertions, 116 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index ce4395e0f..b6392249a 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -21,23 +21,38 @@ #include "drivers/FPGA/FPGA_SPI_Comm.h" #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h" +#include <driverlib/timer.h> +#include <inc/hw_ints.h> +#include <inc/hw_memmap.h> + + bool Winder_ScrewHoming = false; -bool Winder_Active = false; +#define MAX_WINDER_SPEED_CALCULATION 100 uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag); uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue); bool ScrewCurrentDirection = false; //holds current screw direction -bool BusyfirstCall = true; //Ignores first call after activating the screw - too early -double ScrewSpeed = 0; //ScrewSpeed -uint32_t ScrewControlId = 0xFF; +uint32_t ScrewDirectionChangeCounter = 1; //holds the current number of runs of the screw - will be used to build the cone +uint32_t CalculationDirectionChangeCounter = 1; //holds the current number of runs of the screw - will be used to build the cone +uint32_t WinderMotorSpeed[MAX_WINDER_SPEED_CALCULATION]; +uint8_t WinderMotorSpeedCounter=0; +uint32_t WinderMotorSpeedRollOver=0; +double ScrewSpeed = 0; +double ScrewRunningTime = 0; uint32_t ScrewNumberOfSteps = 0; //holds the current number of steps for the next screw run - will be used to build the cone -uint32_t DirectionChangeCounter = 0; //holds the current number of runs of the screw - will be used to build the cone +bool SCREW_TimerActivated = false; +//ScrewSpeed +uint32_t ScrewControlId = 0xFF; InternalWinderConfigStruc InternalWinderCfg = {0}; +void ScrewTimerInterruptInit(void); +void ScrewsStopControlTimer (void); +void ScrewsStartControlTimer (void); uint32_t Winder_Init(void) { + ScrewTimerInterruptInit(); return OK; } @@ -73,9 +88,9 @@ uint32_t Winder_Prepare(void) { uint32_t status = 0; //JobTicket* JobTicket = JobDetails; - Winder_Active = true; //float process_speed = JobTicket->processparameters->dyeingspeed; - double ScrewSpeed = 1000;//(process_speed*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius); // we will use pulley radius of the screw for this purpose, as of now + double ScrewSpeed = 1500;//(process_speed*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius); // we will use pulley radius of the screw for this purpose, as of now + //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses); //REPORT_MSG(ScrewSpeed, "Winder_Prepare"); /* * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop. @@ -84,12 +99,12 @@ uint32_t Winder_Prepare(void) */ if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT) { - REPORT_MSG(LIMIT, "Winder_Prepare at limit"); + //REPORT_MSG(LIMIT, "Winder_Prepare at limit"); Winder_PrepareStage2(0,0); } else { - REPORT_MSG(ScrewSpeed, "Winder_Prepare"); + //REPORT_MSG(ScrewSpeed, "Winder_Prepare"); Winder_ScrewHoming = true; //REPORT_MSG(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, "Winder_Prepare move to limit"); status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, Winder_PrepareStage2); @@ -107,7 +122,7 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue) uint32_t status=OK; uint32_t numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; - MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses); + //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses); //REPORT_MSG(numOfSteps, "Winder_PrepareStage2"); REPORT_MSG(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].maxfrequency, "Winder_PrepareStage2"); @@ -119,16 +134,15 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue) uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag) { - SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW); //set this point as the spool home + //SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW); //set this point as the spool home + //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,temp_MaxFrequency); ScrewCurrentDirection = false; - BusyfirstCall = true; ScrewSpeed = 0; ScrewControlId = 0xFF; ScrewNumberOfSteps = 0; - DirectionChangeCounter = 0; REPORT_MSG(BusyFlag, "Winder_ScrewAtOffsetCallback"); - + MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Soft_Hiz); //per L6470 errata between mov and run commands PrepareReady(Module_Winder, ModuleDone); return OK; } @@ -137,15 +151,6 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag) * this is the main operational function of the screw - run back and forth until stopped */ /* - -bool ScrewCurrentDirection = false; -bool BusyfirstCall = true; -double ScrewSpeed = 0; -uint32_t ScrewControlId = 0xFF; -uint32_t ScrewNumberOfSteps = 0; -uint32_t DirectionChangeCounter = 0; -*/ -/* InternalWinderCfg.segmentoffsetpulses = request->segmentoffsetpulses; InternalWinderCfg.spoolbackingrate = request->backingrate; InternalWinderCfg.startoffsetpulses = request->startoffsetpulses; @@ -169,97 +174,68 @@ InternalWinderCfg.segmentoffsetpulses numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; */ -uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction) +uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag) { - uint32_t NumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; + uint32_t Steps,i,winderspeed=0; + double temp; + uint32_t WinderReferenceSpeed = OriginalMotorSpd_2PPS[WINDER_MOTOR]; float screw_horizontal_speed = 0; float RotationsPerSecond; - if (Counter) + WinderMotorSpeed[WinderMotorSpeedCounter] = CurrentControlledSpeed[WINDER_MOTOR]; + if (WinderMotorSpeedCounter++>=MAX_WINDER_SPEED_CALCULATION) { - if (Counter%InternalWinderCfg.spoolbackingrate == 1) - { - NumberOfSteps -= (Counter/InternalWinderCfg.spoolbackingrate); - REPORT_MSG(ScrewNumberOfSteps, "Head Backing"); - ScrewNumberOfSteps--; - /* - screw_horizontal_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_horizontal_speed*RotationsPerSecond; - - MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); - REPORT_MSG(ScrewSpeed, "CalculateNumberOfSteps"); - CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed; - */ - } - if ((Counter%InternalWinderCfg.SpoolBottomBackingRate == 0)||(Counter%InternalWinderCfg.SpoolBottomBackingRate == 1)) - { - if (direction != MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize) - { - NumberOfSteps += (Counter/InternalWinderCfg.SpoolBottomBackingRate); - ScrewNumberOfSteps++; - REPORT_MSG(ScrewNumberOfSteps, "Bottom Backing"); - } - - } + WinderMotorSpeedCounter=0; + WinderMotorSpeedRollOver++; } - return NumberOfSteps; -} -uint32_t wtick=0,prevwtick = 0; -uint32_t wgap[100]; -uint32_t wgap_counter=0; -uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag) -{ - uint32_t Steps; - if (BusyfirstCall) - { - BusyfirstCall = false; + if (ScrewDirectionChangeCounter == CalculationDirectionChangeCounter) return OK; - } -// if (Winder_Active == false) -// return OK; - wtick++; - //ScrewCurrentDirection: false moves out, true moves home - if (BusyFlag == NOTBUSY) + + ScrewCurrentDirection = 1-ScrewCurrentDirection; + CalculationDirectionChangeCounter++; + + if (ScrewCurrentDirection == 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize) //next time going out { - if (wtick-prevwtick>10) + if ((CalculationDirectionChangeCounter/2)%InternalWinderCfg.spoolbackingrate == 0) { - wgap[wgap_counter++] = wtick-prevwtick; - prevwtick = wtick; - if (wgap_counter >=99) - wgap_counter = 0; - } - else - return OK; - CalculateNumberOfSteps (DirectionChangeCounter++, ScrewCurrentDirection); - Steps = ScrewNumberOfSteps; - if (ScrewCurrentDirection == false) - { - ScrewCurrentDirection = true; - } - else - { - ScrewCurrentDirection = false; + ScrewNumberOfSteps--; + REPORT_MSG(ScrewNumberOfSteps, "Head Backing ScrewNumberOfSteps"); + } -/* if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT) + } + else //next time going back + { + if ((CalculationDirectionChangeCounter/2)%InternalWinderCfg.SpoolBottomBackingRate == 0) { - 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"); + ScrewNumberOfSteps++; + REPORT_MSG(ScrewNumberOfSteps, "Bottom Backing ScrewNumberOfSteps"); } - else if (FPGA_Read_limit_Switches(GPI_LS_SCREW_LEFT)==LIMIT) + } + if (WinderMotorSpeedRollOver) + { + for (i=0;i<MAX_WINDER_SPEED_CALCULATION;i++) { - ScrewCurrentDirection = (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize); - REPORT_MSG(LIMIT, "Winder at left limit"); + winderspeed+=WinderMotorSpeed[i]; } -*/ - MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,Steps); //process: set point 0, set max speed, move to the specified length, return back. - REPORT_MSG(ScrewCurrentDirection, "ScrewDirectionChange"); + winderspeed/=MAX_WINDER_SPEED_CALCULATION; + LOG_ERROR(winderspeed, "WinderSpeedUpdated"); + WinderReferenceSpeed = winderspeed; + } + screw_horizontal_speed = ScrewNumberOfSteps / InternalWinderCfg.NumberOfRotationPerPassage; + RotationsPerSecond = WinderReferenceSpeed / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround; + ScrewSpeed = screw_horizontal_speed*RotationsPerSecond; + CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed; + temp = SYS_CLK_FREQ; + temp *= ScrewNumberOfSteps; + temp /= ScrewSpeed; + if (ScrewRunningTime != temp) + { + LOG_ERROR(temp , "new winder speed"); } + ScrewRunningTime = temp;//(SYS_CLK_FREQ*Steps)/ScrewSpeed; + + /********************************************************************************/ return OK; } @@ -275,7 +251,7 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId) float screw_horizontal_speed = 0; float RotationsPerSecond; char Message[80]; - + double temp = 0; if (dyeingspeed == 0) { LOG_ERROR (dyeingspeed," job speed zero"); @@ -292,47 +268,61 @@ 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_horizontal_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; + //RotationsPerSecond = dyeingspeed / (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulleyradius * PI); + RotationsPerSecond = OriginalMotorSpd_2PPS[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_horizontal_speed*RotationsPerSecond; - MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); + //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); usnprintf(Message, 80, "SCREW speed Rot/sec %d horizon %d pulses %d",(int)RotationsPerSecond,(int)screw_horizontal_speed,(int)ScrewSpeed); //LOG_ERROR(segmentfirst_speed,Message); //Report(Message,__FILE__,__LINE__,Dispenser_i,RpWarning,segmentfirst_speed,0); SendJobProgress(0.0,0,false, Message); CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed; + OriginalMotorSpd_2PPS[SCREW_MOTOR] = ScrewSpeed; //screw_horizontal_speed = InternalWinderCfg.milimetersperrotation // * 2. determine optimal micro-step setting // * 3. calculate cart travel length from winding parameters // * 4. start move of travel length // * 5. register motor nBusy callback. this callback will flip between move(traverse length, hardstop) and goto(0), with handline og the coneshape and adjusting maxspeed - ScrewNumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; - MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back. - wtick = 0;prevwtick = 0; - ScrewControlId = AddControlCallback(ScrewDirectionChange, eOneMillisecond,MotorControlGetnBusyState,(IfTypeMotors*0x100+HARDWARE_MOTOR_TYPE__MOTO_SCREW), HARDWARE_MOTOR_TYPE__MOTO_SCREW, 0); - - // MotorSetSpeedWithCallback (HARDWARE_MOTOR_TYPE__MOTO_SCREW, screw_horizontal_speed,WinderPresegmentReady); - //in a callback: calculate backing rate for top and bottom, update point 0, update passing length, call the appropriate move to 0 / move; - - } + ScrewNumberOfSteps = InternalWinderCfg.segmentoffsetpulses;//*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; + temp = SYS_CLK_FREQ; + temp *= InternalWinderCfg.segmentoffsetpulses; + temp /= ScrewSpeed; + ScrewRunningTime = temp;//(SYS_CLK_FREQ*InternalWinderCfg.segmentoffsetpulses)/ScrewSpeed; + LOG_ERROR(ScrewNumberOfSteps,"Winder pre segment - ScrewNumberOfSteps"); + LOG_ERROR(ScrewRunningTime,"Winder pre segment - ScrewRunningTime"); +// MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection); + //ScrewDirection = 1-ScrewDirection; + LOG_ERROR(ScrewSpeed,"Winder pre segment - ScrewSpeed"); + //MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); + ScrewsStartControlTimer (); + ScrewControlId = AddControlCallback(Screw100msecDirectionChange, eHundredMillisecond,TemplateDataReadCBFunction,0,0,0); + ScrewDirectionChangeCounter = 1; + CalculationDirectionChangeCounter = 1; + memset (WinderMotorSpeed,0,sizeof(uint32_t) *MAX_WINDER_SPEED_CALCULATION); + WinderMotorSpeedCounter=0,WinderMotorSpeedRollOver=0; + } PreSegmentReady(Module_Winder,ModuleDone); return OK; } uint32_t Winder_End(void) { - int pend; //stop screw ScrewNumberOfSteps = 0; -// Winder_Active = false; if (ScrewControlId != 0xFF) - RemoveControlCallback(ScrewControlId,ScrewDirectionChange); + { + RemoveControlCallback(ScrewControlId,Screw100msecDirectionChange); + ScrewControlId = 0xFF; + } CurrentControlledSpeed[SCREW_MOTOR] = 0; - pend = MillisecFlushMsgQ(HARDWARE_MOTOR_TYPE__MOTO_SCREW); - return MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); + ScrewsStopControlTimer(); + //move the cart to the edge so the spool can be easily replaced + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, 1000, GPI_LS_SCREW_RIGHT, NULL); + + return OK; } void Winder_ScrewHomeLimitSwitchInterrupt(void) { @@ -351,3 +341,62 @@ void Winder_ScrewOutLimitSwitchInterrupt(void) MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out } +uint32_t Screw_timerBase = TIMER3_BASE; //Timer handle + +bool Screw_Restart = false; +void ScrewTimerInterruptInit(void) +{ + ROM_TimerConfigure(Screw_timerBase, TIMER_CFG_PERIODIC); // 32 bits Timer + ROM_TimerEnable(Screw_timerBase, TIMER_A); + ROM_IntEnable(INT_TIMER3A); + ROM_TimerIntEnable(Screw_timerBase, TIMER_TIMA_TIMEOUT); + Screw_Restart = false; + +} +void ScrewsStopControlTimer (void) +{ + SCREW_TimerActivated = false; + ROM_TimerDisable(Screw_timerBase, TIMER_A); + //ROM_IntDisable(INT_TIMER3A); + //ROM_TimerIntDisable(Screw_timerBase, TIMER_TIMA_TIMEOUT); +} +void ScrewsStartControlTimer (void) +{ + if (SCREW_TimerActivated == true) + return; + SCREW_TimerActivated = true; + //ROM_TimerConfigure(Screw_timerBase, TIMER_CFG_PERIODIC); // 32 bits Timer + TimerEnable(Screw_timerBase, TIMER_A); + ROM_IntEnable(INT_TIMER3A); + ROM_TimerIntEnable(Screw_timerBase, TIMER_TIMA_TIMEOUT); + ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)1200000/*10 millisec*/ ); + Report("ScrewsStartControlTimer direction,speed ", __FILE__,__LINE__,ScrewCurrentDirection, RpMessage, ScrewSpeed, 0); + return; +} + +void ScrewTimerInterrupt(int ARG0) +{ + ROM_TimerIntClear(Screw_timerBase, TIMER_TIMA_TIMEOUT); // Clear the timer interrupt + ROM_IntMasterDisable(); + //ScrewDirectionChange(0,NOTBUSY); + + if (SCREW_TimerActivated == true) + { + MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection); +// ScrewCurrentDirection = 1-ScrewCurrentDirection; + MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); + ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)ScrewRunningTime); + ScrewDirectionChangeCounter++; + } + else + { + TimerDisable(Screw_timerBase, TIMER_A); + } + //Report("ScrewTimerInterrupt dir, duration, speed", __FILE__,ScrewCurrentDirection,ScrewRunningTime, RpMessage, ScrewSpeed, 0); + // + // Enable all interrupts. + // + ROM_IntMasterEnable(); + return ; + +} |