diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread')
5 files changed, 28 insertions, 100 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h index 57b7a9d99..db3bfc46e 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h @@ -55,7 +55,6 @@ extern HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS]; extern int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM]; extern double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM]; -extern double DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM]; uint32_t InternalWinderConfigMessage(HardwareWinder* request); @@ -66,5 +65,8 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request); uint32_t DancerConfigMessage(HardwareDancer * request); +uint32_t ThreadPrepareState(void *JobDetails); +uint32_t Winder_Prepare(void); + #endif //MODULES_THREAD_THREAD_H_ diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index 164e5eb23..b61247fe4 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -9,15 +9,9 @@ #include "Drivers/Peripheral_GPIO/GPIO.h" #include "drivers/Motors/Motor.h" #include "StateMachines/Printing/PrintingSTM.h" - -#include "drivers/FPGA/FPGA.h" - -#include "drivers/FPGA/FPGA_SPI_Comm.h" -#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h" - bool Winder_ScrewHoming = false; uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps); -uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue); +uint32_t Winder_PrepareStage2(void); double ScrewSpeed = 0; @@ -25,77 +19,31 @@ uint32_t Winder_Init(void) { return OK; } -uint32_t Winder_Test2(uint32_t deviceID, uint32_t ReadValue); -uint32_t Winder_Test1(uint32_t deviceID, uint32_t ReadValue) -{ - uint32_t status; - uint32_t numOfSteps = InternalWinderCfg.segmentoffsetpulses; - uint32_t position; - /* - * 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. - * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done - * report ready to the job STM - */ - MotorGetPositionFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_SCREW); - //deley TODO - SysCtlDelay(100000); - position = Fpga_Spi[HARDWARE_MOTOR_TYPE__MOTO_SCREW].RX_MISO;//MotorGetPositionFromFPGA_Res(Motor_Id); - - status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI_LS_SCREW_LEFT, Winder_Test2); -// status |= MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback); - //set motor location 0 here - return status; - -} -uint32_t Winder_Test2(uint32_t deviceID, uint32_t ReadValue) -{ - uint32_t status; - uint32_t numOfSteps = InternalWinderCfg.segmentoffsetpulses; - uint32_t position; - /* - * 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. - * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done - * report ready to the job STM - */ - MotorGetPositionFromFPGA(HARDWARE_MOTOR_TYPE__MOTO_SCREW); - //deley TODO - SysCtlDelay(100000); - position = Fpga_Spi[HARDWARE_MOTOR_TYPE__MOTO_SCREW].RX_MISO;//MotorGetPositionFromFPGA_Res(Motor_Id); - - status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI_LS_SCREW_RIGHT, Winder_Test1); - //status |= MotorMoveSteps (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback); - //set motor location 0 here - return status; - -} -uint32_t Winder_Prepare(void *JobDetails) +uint32_t Winder_Prepare(void) { uint32_t status = 0; - JobTicket* JobTicket = JobDetails; - - float process_speed = JobTicket->processparameters->dyeingspeed; // double ScrewSpeed = (50 * MotorsCfg[SCREW_MOTOR].pulseperround * MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius); - double ScrewSpeed = (process_speed * MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius); + double ScrewSpeed = (50 * MotorsCfg[SCREW_MOTOR].pulseperround)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius); /* * 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. * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done * report ready to the job STM */ - if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT) + if (PollGPIO(GPI113_LS_SCREW_RIGHT)) { //go to prepare stage 2 - Winder_PrepareStage2(0,0); + Winder_PrepareStage2(); } else { Winder_ScrewHoming = true; -// status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, Winder_PrepareStage2); - status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed/10, GPI_LS_SCREW_RIGHT, Winder_Test1); + status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1);//make sur to move the cart home + status |= MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewSpeed); } return status; } -uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue) +uint32_t Winder_PrepareStage2(void) { uint32_t status; uint32_t numOfSteps = InternalWinderCfg.segmentoffsetpulses; @@ -104,8 +52,7 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue) * 2. move back x steps - according to thehw specifications and bobine definitions in the job. move for a predefined number of steps. get a callback when done * report ready to the job STM */ - status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,(1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize));//make sur to move the cart out -// status |= MotorMove(HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback); + status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,0);//make sur to move the cart out //status |= MotorMoveSteps (HARDWARE_MOTOR_TYPE__MOTO_SCREW, numOfSteps, Winder_ScrewAtOffsetCallback); //set motor location 0 here return status; @@ -124,7 +71,7 @@ uint32_t Winder_Presegment(void *JobDetails) float RotationsPerSecond; // double ScrewSpeed = (process_speed * MotorsCfg[SCREW_MOTOR].pulseperround * MotorsCfg[SCREW_MOTOR].microstep)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius); - double ScrewSpeed = (process_speed * MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulseperround)/(2*PI* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius); + double ScrewSpeed = (process_speed * MotorsCfg[SCREW_MOTOR].pulseperround)/(2*PI* MotorsCfg[SCREW_MOTOR].pulleyradius); /*typedef struct { @@ -169,12 +116,12 @@ void Winder_ScrewHomeLimitSwitchInterrupt(void) { StopMotor(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Stop); //stop ASAP } - status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out + status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[SCREW_MOTOR].directionthreadwize);//make sure to move the cart out } void Winder_ScrewOutLimitSwitchInterrupt(void) { 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 + status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[SCREW_MOTOR].directionthreadwize);//make sure to move the cart out } uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps) { diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h index 28a2346c1..ea75bffa3 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h @@ -18,7 +18,7 @@ uint32_t ThreadInitialTestStub(); //uint32_t MotorPidRequestMessage(HardwarePidControl* request); uint32_t Winder_Init(void); -uint32_t Winder_Prepare(void *JobDetails); +uint32_t Winder_Prepare(void); uint32_t Winder_Presegment(void *JobDetails); uint32_t Winder_End(void); diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c index e2d492b47..0e276fe92 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -17,7 +17,6 @@ HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0}; 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}; -double DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM] = {0}; InternalWinderConfigStruc InternalWinderCfg = {0}; HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS] = {0}; @@ -98,11 +97,10 @@ uint32_t MotorPidRequestMessage(HardwarePidControl* request) MotorSamples[Motor_i][i] = 0; //reset the samples value for control beginning 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=(100*(temp-1)*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm); NormalizedErrorCoEfficient[Motor_i] = NormalizedErrorCoEfficient[Motor_i] / temp; // uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES]; - DancerStopActivityLimit[Motor_i] = ((1<<(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].resolutionbits)*(DancersCfg[ThreadMotorIdToDancerId[Motor_i]].maximalmovementmm)*2)); - DancerStopActivityLimit[Motor_i] = DancerStopActivityLimit[Motor_i]/(2*PI*DancersCfg[ThreadMotorIdToDancerId[Motor_i]].armlength); + return OK; } uint32_t DancerConfigMessage(HardwareDancer * request) diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 56e1bf844..92bfa9c92 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -22,7 +22,6 @@ #include "drivers/Heater/TemperatureSensor.h" #include "drivers/Heater/Heater.h" #include "drivers/Motors/Motor.h" -#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h" #include "modules/heaters/heaters.h" ////////////////////////////////State machine operation//////////////////////////////////// @@ -38,7 +37,7 @@ uint32_t ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF}; uint32_t SpeedControlId=0xFF; int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0}; -uint32_t JobCounter = 0; + typedef struct { bool m_isEnabled; @@ -67,8 +66,6 @@ ProcessedLengthFunc ProcessedLengthFuncPtr = NULL; void ThreadSegmentEnded(void); void ThreadInterSegmentEnded(void); void ThreadDistanceToSpoolEnded(void); - -double KeepNormalizedError = 0; ////////////////////////Slow Motor State//////////////////////////////////// //uint32_t ThreadPreSegmentState(void *JobDetails); @@ -168,7 +165,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) #warning control disabled CurrentProcessedLength+=length; - +#warning control disabled PosDif[MotorDataIndex] = CurrentPosition; //PosDif[MotorDataIndex] = positionDiff; MotorSentData[MotorDataIndex] = length; @@ -179,9 +176,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) pooler_counter++; if (pooler_counter%10 == 0) { - //SendJobProgress(CurrentProcessedLength/CurrentRequestedLength,CurrentSegmentId,false); - SendJobProgress(/*KeepNormalizedError*/MotorControlConfig[index].m_calculatedError,CurrentSegmentId,false); - + SendJobProgress(CurrentProcessedLength/CurrentRequestedLength,CurrentSegmentId,false); } if (pooler_counter>=100) { @@ -192,6 +187,8 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) } if (CurrentProcessedLength>=CurrentRequestedLength ) { + SendJobProgress(100,0,true); + #warning handle job wit several segments!!! // segment/intersegment/distance to spool finished if (ProcessedLengthFuncPtr) ProcessedLengthFuncPtr(); @@ -283,14 +280,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { DancerId = ThreadMotorIdToDancerId[index]; if (ReadValue < 10) - return OK; + return; TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; - if (index == POOLER_MOTOR) - { - //pooler dancer is right sided: data is opposite + if (index == POOLER_MOTOR) //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); - JobCounter++; - } //TranslatedReadValue = 0;//test MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; @@ -298,20 +291,12 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++) avreageSampleValue += MotorSamples[index][i]; avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode; - //Stop Execution if the dancer moves too much -#warning need to disable this in the first second of the job execution - if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond)) - { - EndState(CurrentJob); - } NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index]; MotorControlConfig[index].m_mesuredParam = 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) //feeder unit handles errors opposite to left unit + if (index == FEEDER_MOTOR) //feeder unit handles errors opposite to left unit MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError); - else - KeepNormalizedError = NormalizedError; calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; if (abs(calculated_speed-CurrentControlledSpeed[index])>5) { @@ -324,7 +309,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) pooler_counter++; if (pooler_counter>=1000) { - //float error_integered = MotorControlConfig[index].m_calculatedError*1000; + float error_integered = MotorControlConfig[index].m_calculatedError*1000; /*{ "HeaterGroupId": 0, "Zone1Temp": 80, @@ -339,7 +324,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) }*/ //HeatingTestSendResonse(0, false,true,true, MotorDriverRequest[22].Speed,MotorDriverRequest[18].Speed,MotorDriverRequest[15].Speed,MotorDriverRequest[3].Speed, "MotorSpeed"); - HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/_speed,OriginalMotorSpd_2PPS[index]/*(int)error_integered*/,MotorControlConfig[index].m_calculatedError,ReadValue, "FeederSpeed"); + HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/_speed,OriginalMotorSpd_2PPS[index]/*(int)error_integered*/,calculated_speed,ReadValue, "FeederSpeed"); pooler_counter = 0; } } @@ -365,8 +350,6 @@ bool InitialProcess = false; { int Motor_i, HW_Motor_Id, Pid_Id; CurrentSegmentId = 0; - - JobCounter = 0; //start thread control for all motors for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++) { @@ -492,7 +475,6 @@ uint32_t ThreadPreSegmentState(void *JobDetails) MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 10); //#warning rocker disabled -// MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 0, GPI_LS_RLOADMOTOR_UP, EndState); //TODO // activate control fr all motors //set speed for both rocker motors @@ -504,7 +486,6 @@ uint32_t ThreadPreSegmentState(void *JobDetails) } else { - ThreadUpdateProcessLength (0,(void *)NULL); PreSegmentReady(Module_Thread,ModuleDone); InitialProcess = false; } |