diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread')
4 files changed, 122 insertions, 36 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h index dad7a1db6..7780287fb 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h @@ -48,7 +48,7 @@ extern HardwarePidControlType ThreadMotorIdToControlId[MAX_THREAD_MOTORS_NUM]; extern HardwareMotor MotorsCfg[NUM_OF_MOTORS]; extern HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM]; -extern InternalWinderConfigStruc InternalWinderCfg; +//extern InternalWinderConfigStruc InternalWinderCfg; extern HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS]; #define MAX_CONTROL_SAMPLES 10 diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index a119b0b16..33086b636 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -1,7 +1,7 @@ /* * Thread_Winder.c * - * Created on: 25 áîøõ 2018 + * Created on: 25 áîøõ 2018 * Author: shlomo */ #include"include.h" @@ -11,6 +11,8 @@ #include "StateMachines/Printing/PrintingSTM.h" #include "Modules/Control/Control.h" +#include "Common/report/report.h" + #include "drivers/FPGA/FPGA.h" #include "drivers/FPGA/FPGA_SPI_Comm.h" @@ -27,10 +29,35 @@ uint32_t ScrewControlId = 0xFF; 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 +InternalWinderConfigStruc InternalWinderCfg = {0}; + uint32_t Winder_Init(void) { return OK; } + + +uint32_t InternalWinderConfigMessage(HardwareWinder* request) +{ + uint32_t status = PASSED; + + InternalWinderCfg.milimetersperrotation = request->millimeterperrotation; + + return status; +} +uint32_t InternalWindingConfigMessage(JobSpool* request) +{ + uint32_t status = PASSED; + + InternalWinderCfg.segmentoffsetpulses = request->segmentoffsetpulses; + InternalWinderCfg.spoolbackingrate = request->backingrate; + InternalWinderCfg.startoffsetpulses = request->startoffsetpulses; + InternalWinderCfg.SpoolBottomBackingRate = request->bottombackingrate; + InternalWinderCfg.NumberOfRotationPerPassage = request->rotationsperpassage; + InternalWinderCfg.diameter = request->diameter; + + return status; +} /* *uint32_t Winder_Prepare(void *JobDetails) @@ -107,7 +134,56 @@ uint32_t ScrewControlId = 0xFF; uint32_t ScrewNumberOfSteps = 0; uint32_t DirectionChangeCounter = 0; */ +/* + InternalWinderCfg.segmentoffsetpulses = request->segmentoffsetpulses; + InternalWinderCfg.spoolbackingrate = request->backingrate; + InternalWinderCfg.startoffsetpulses = request->startoffsetpulses; + InternalWinderCfg.SpoolBottomBackingRate = request->bottombackingrate; + InternalWinderCfg.NumberOfRotationPerPassage = request->rotationsperpassage; + * +Calculate the number of steps. +Initial home position = ... +Initial out movement = ScrewNumberOfSteps +if DirectionChangeCounter %= backingrate : reduce one from the ScrewNumberOfSteps +if DirectionChangeCounter %= bottombackingrate && direction was out: ADD one to the ScrewNumberOfSteps, +// WRONG? if the flag is raised - lower it and reduce one from the ScrewNumberOfSteps +30:100 - 70 +30:99 - 69 +30:98 - 68 +29:98 - 69 +29:97 -68 +InternalWinderCfg.segmentoffsetpulses + int32_t backingrate; + int32_t bottombackingrate; + +numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; +*/ +uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction) +{ + uint32_t NumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep; + if (Counter) + { + if (Counter%InternalWinderCfg.spoolbackingrate == 0) + { + NumberOfSteps -= (Counter/InternalWinderCfg.spoolbackingrate); + REPORT_MSG(ScrewNumberOfSteps, "Head Backing"); + ScrewNumberOfSteps--; + } + if ((Counter%InternalWinderCfg.SpoolBottomBackingRate == 0)||(Counter%InternalWinderCfg.SpoolBottomBackingRate == 0)) + { + if (direction != MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize) + { + NumberOfSteps += (Counter/InternalWinderCfg.SpoolBottomBackingRate); + ScrewNumberOfSteps++; + REPORT_MSG(ScrewNumberOfSteps, "Bottom Backing"); + } + + } + } + return CalculateNumberOfSteps; + +} uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag) { if (BusyfirstCall) @@ -118,6 +194,7 @@ uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag) //ScrewCurrentDirection: false moves out, true moves home if (BusyFlag == NOTBUSY) { + CalculateNumberOfSteps (DirectionChangeCounter++, ScrewCurrentDirection); if (ScrewCurrentDirection == false) { ScrewCurrentDirection = true; @@ -127,7 +204,6 @@ 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. - DirectionChangeCounter++; } /* * calculate new ScrewSpeed and call MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed); @@ -197,6 +273,7 @@ uint32_t Winder_End(void) void Winder_ScrewHomeLimitSwitchInterrupt(void) { uint32_t status; + //handle glitch - send information to the next time that the motor stops if (Winder_ScrewHoming) { MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz); //stop ASAP @@ -205,6 +282,7 @@ void Winder_ScrewHomeLimitSwitchInterrupt(void) } void Winder_ScrewOutLimitSwitchInterrupt(void) { + //handle glitch - send information to the next time that the motor stops uint32_t status; status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out } diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c index 3c9adbc31..c5f45da78 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -18,35 +18,10 @@ int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0}; int MotorSamplePointer[MAX_THREAD_MOTORS_NUM] = {0}; double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM] = {0}; int DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM] = {0}; -InternalWinderConfigStruc InternalWinderCfg = {0}; HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS] = {0}; HardwarePidControlType ThreadMotorIdToControlId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_PID_CONTROL_TYPE__MotorFeeder,HARDWARE_PID_CONTROL_TYPE__MotorDryer,HARDWARE_PID_CONTROL_TYPE__MotorPooler,HARDWARE_PID_CONTROL_TYPE__MotorWinder,0}; - - -uint32_t InternalWinderConfigMessage(HardwareWinder* request) -{ - uint32_t status = PASSED; - - InternalWinderCfg.milimetersperrotation = request->millimeterperrotation; - - return status; -} -uint32_t InternalWindingConfigMessage(JobSpool* request) -{ - uint32_t status = PASSED; - - InternalWinderCfg.segmentoffsetpulses = request->segmentoffsetpulses; - InternalWinderCfg.spoolbackingrate = request->backingrate; - InternalWinderCfg.startoffsetpulses = request->startoffsetpulses; - InternalWinderCfg.SpoolBottomBackingRate = request->bottombackingrate; - InternalWinderCfg.NumberOfRotationPerPassage = request->rotationsperpassage; - InternalWinderCfg.diameter = request->diameter; - - return status; -} - //******************************************************************************************************************** uint32_t MotorsConfigMessage(HardwareMotor * request) { diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 0701a2aac..d9aeb503a 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -70,6 +70,8 @@ ProcessedLengthFunc ProcessedLengthFuncPtr = NULL; void ThreadSegmentEnded(void); void ThreadInterSegmentEnded(void); void ThreadDistanceToSpoolEnded(void); +uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue); +void SetOriginMotorSpeed(float process_speed); double KeepNormalizedError = 0; bool ThreadControlActive = false; @@ -248,12 +250,37 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue) } return OK; } -double calculatedError[1000]; -int readValue[1000]; -int calculatedspeed[1000]; +/*double calculatedError[100]; +double eNormalizedError[100]; +int readValue[100]; +int TranslatedreadValue[100]; +int AveragereadValue[100]; +int calculatedspeed[100]; int controlIndex = 0; int32_t KeepReadValue = 0; - +void testDancersControl() +{ + int mm20,mm10,mm5,mm2,mm1; + mm20 = (20*DancerStopActivityLimit[FEEDER_MOTOR])/(DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].maximalmovementmm*2); + mm2 = mm20/10; + mm5 = mm20/4; + mm10 = mm20/2; + mm1 = mm20/20; + ThreadControlActive = true; + SetOriginMotorSpeed(30.0); + ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm20); + ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm10); + ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm5); + ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm2); + ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm1); + ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint); + ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm1); + ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm2); + ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm5); + ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm10); + ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm20); + ThreadControlActive = false; +}*/ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //#define MAX_CONTROL_SAMPLES 6 @@ -270,6 +297,8 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) double NormalizedError; char Message[60]; + if (ThreadControlActive == false) + return OK; if (IfIndex>>8 != IfTypeThread) { LOG_ERROR (IfIndex, "Wrong Interface type"); @@ -339,18 +368,21 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) //KeepNormalizedError = NormalizedError; } calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; - if (index == POOLER_MOTOR) + /*if (index == FEEDER_MOTOR) { if (KeepReadValue != TranslatedReadValue) { + eNormalizedError[controlIndex] = NormalizedError; calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError; - readValue[controlIndex] = TranslatedReadValue; + readValue[controlIndex] = ReadValue; + TranslatedreadValue[controlIndex] = TranslatedReadValue; + AveragereadValue[controlIndex] = avreageSampleValue; calculatedspeed[controlIndex] = calculated_speed; controlIndex++; - if (controlIndex >= 999) controlIndex = 0; + if (controlIndex >= 99) controlIndex = 0; KeepReadValue = TranslatedReadValue; } - } + }*/ if (abs(calculated_speed-CurrentControlledSpeed[index])>5) { CurrentControlledSpeed[index] = calculated_speed; @@ -482,6 +514,7 @@ bool InitialProcess = false; // else if ((Motor_i == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||(Motor_i == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)) // AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); } + //testDancersControl(); PrepareReady(Module_Thread,ModuleDone); //set 3 dancers to the profile positions InitialProcess = true; |