From 5e67333d1231c2020e8d4ea062fc74418be9f3e5 Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Tue, 29 May 2018 18:50:18 +0300 Subject: State machine and job handling. motor speed/position is not clean --- .../Embedded/Modules/Thread/Thread_ex.h | 2 + .../Embedded/Modules/Thread/Thread_print.c | 141 ++++++++++++++------- 2 files changed, 100 insertions(+), 43 deletions(-) (limited to 'Software/Embedded_SW/Embedded/Modules/Thread') diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h index e957b0efc..ea75bffa3 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h @@ -20,6 +20,8 @@ uint32_t ThreadInitialTestStub(); uint32_t Winder_Init(void); uint32_t Winder_Prepare(void); uint32_t Winder_Presegment(void *JobDetails); +uint32_t Winder_End(void); + #endif diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 1e2203abb..9a7e8cf88 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -34,6 +34,8 @@ uint32_t CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0}; TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,HARDWARE_MOTOR_TYPE__MOTO_SCREW}; HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS}; uint32_t ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF}; +uint32_t SpeedControlId=0xFF; + int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0}; typedef struct @@ -81,7 +83,8 @@ void ThreadDistanceToSpoolEnded(void); uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read) { uint32_t Time_Pass; - #define MAX_COUNTER 0x3FFF //14 bits +// #define MAX_COUNTER 0x3FFF //14 bits + #define MAX_COUNTER 0x3FFFFF //22 bits if (Current_Read < Previous_Read) @@ -100,6 +103,64 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea * * **************************************************************************************/ +void ThreadUpdateProcessLength (double length, void *Funcptr) +{ + CurrentRequestedLength = length*100;//Centimetres + CurrentProcessedLength = 0; + PreviousPosition = 0; + CurrentPosition = 0; + ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr; +} +uint32_t MotorSentData[1000] = {0}; +uint32_t PosDif[1000] = {0}; +uint32_t tick[1000] = {0}; +uint32_t initialpos = 0xFFFF; + + +int MotorDataIndex = 0; + +uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) +{ + uint32_t positionDiff = 0; + double length = 0.0; + int index = MAX_THREAD_MOTORS_NUM; + if (IfIndex>>8 != IfTypeThread) + { + LOG_ERROR (IfIndex, "Wrong Interface type"); + return 0xFFFFFFFF; + } + index = IfIndex&0xFF; + if (index != FEEDER_MOTOR) + { + LOG_ERROR (IfIndex, "Wrong Motor"); + return 0xFFFFFFFF; + } + CurrentPosition = MotorGetPositionFromFPGA_Res(ThreadMotorIdToMotorId[index]); + if (initialpos == 0xFFFF) + initialpos = CurrentPosition; + positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition); + positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; + PreviousPosition = CurrentPosition; + + // total length = (position diff / full cycle) * pulley perimeter + //(positionDiff/pulseperround)*((2*PI*motor_Radius) + + length = (positionDiff/MotorsCfg[ThreadMotorIdToMotorId[index]].pulseperround)*(2*PI*MotorsCfg[ThreadMotorIdToMotorId[index]].pulleyradius); + totalLength+=length; + CurrentProcessedLength+=length; + PosDif[MotorDataIndex] = positionDiff; + MotorSentData[MotorDataIndex] = length; + tick[MotorDataIndex] = UsersysTickGet(); + MotorDataIndex+=1; + if (MotorDataIndex == 999) MotorDataIndex = 0; + if (CurrentProcessedLength>=CurrentRequestedLength ) + { + // segment/intersegment/distance to spool finished + if (ProcessedLengthFuncPtr) + ProcessedLengthFuncPtr(); + } +return OK; +} uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //read value is the dancer angle @@ -140,46 +201,6 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) } return OK; } -void ThreadUpdateProcessLength (double length, void *Funcptr) -{ - CurrentRequestedLength = length; - CurrentProcessedLength = 0; - ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr; -} -uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) -{ - uint32_t positionDiff = 0; - double length = 0.0; - int index = MAX_THREAD_MOTORS_NUM; - if (IfIndex>>8 != IfTypeThread) - { - LOG_ERROR (IfIndex, "Wrong Interface type"); - return 0xFFFFFFFF; - } - index = IfIndex&0xFF; - if (index != FEEDER_MOTOR) - { - LOG_ERROR (IfIndex, "Wrong Motor"); - return 0xFFFFFFFF; - } - CurrentPosition = MotorGetPositionFromFPGA_Res(ThreadMotorIdToMotorId[index]); - positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition); - PreviousPosition = CurrentPosition; - - // total length = (position diff / full cycle) * pulley perimeter - //(positionDiff/pulseperround)*((2*PI*motor_Radius) - - length = (positionDiff/MotorsCfg[ThreadMotorIdToMotorId[index]].pulseperround)*(2*PI*MotorsCfg[ThreadMotorIdToMotorId[index]].pulleyradius); - totalLength+=length; - CurrentProcessedLength+=length; - if (CurrentProcessedLength>=CurrentRequestedLength ) - { - // segment/intersegment/distance to spool finished - if (ProcessedLengthFuncPtr) - ProcessedLengthFuncPtr(); - } -return OK; -} uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //#define MAX_CONTROL_SAMPLES 6 @@ -217,6 +238,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; if (index == POOLER_MOTOR) TranslatedReadValue = (-1*TranslatedReadValue); + //TranslatedReadValue = 0;//test MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode) MotorSamplePointer[index] = 0; @@ -295,13 +317,38 @@ bool InitialProcess = false; MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled - ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); + { + if (SpeedControlId != 0xFF) + { + RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction); + } + SetMotHome(ThreadMotorIdToMotorId[Motor_i]); + SpeedControlId = AddControlCallback(ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); + } if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled + { + if (ControlIdtoMotorId[Motor_i] != 0xFF) + { + RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction); + } ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); + } 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 + { + if (ControlIdtoMotorId[Motor_i] != 0xFF) + { + RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction); + } ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); + } 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 + { + if (ControlIdtoMotorId[Motor_i] != 0xFF) + { + RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction); + } ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); + } /*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,MotorGetSpeed,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0); // continue; @@ -327,7 +374,7 @@ uint32_t ThreadPreSegmentState(void *JobDetails) JobTicket* JobTicket = JobDetails; int Motor_i, HW_Motor_Id; - int process_speed = JobTicket->processparameters->dyeingspeed; + float process_speed = JobTicket->processparameters->dyeingspeed; for (Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) { @@ -389,8 +436,16 @@ uint32_t ThreadSegmentState(void *JobDetails, int SegmentId) uint32_t ThreadEndState(void *JobDetails) { int Motor_i; + if (SpeedControlId != 0xFF) + { + RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction); + } for ( Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++) { + if (ControlIdtoMotorId[Motor_i] != 0xFF) + { + RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction); + } StopMotor(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); } StopMotor(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz); -- cgit v1.3.1