diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread')
5 files changed, 127 insertions, 13 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h index b31c74830..7690b131f 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h @@ -49,7 +49,7 @@ extern HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM]; extern InternalWinderConfigStruc InternalWinderCfg; extern HardwareDancer DancersCfg[MAX_SYSTEM_DANCERS]; -#define MAX_CONTROL_SAMPLES 6 +#define MAX_CONTROL_SAMPLES 10 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]; diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c index 6a05b83ce..83068666b 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c @@ -60,7 +60,7 @@ uint32_t Winder_PrepareStage2(void) uint32_t WinderPresegmentReady(uint32_t deviceID, uint32_t ReadValue) { - return PreSegmentReady(Module_Winder,OK); + return PreSegmentReady(Module_Winder,ModuleDone); } uint32_t Winder_Presegment(void *JobDetails) { @@ -125,13 +125,13 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t NumberOfSteps) { if (NumberOfSteps == InternalWinderCfg.segmentoffsetpulses) { - PrepareReady(Module_Winder, OK); + PrepareReady(Module_Winder, ModuleDone); return OK; } else { //do we want to do something? - PrepareReady(Module_Winder, ERROR); + PrepareReady(Module_Winder, ModuleFail); return ERROR; } diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h index c1d591a38..5f49c7d10 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h @@ -1,12 +1,16 @@ #ifndef MODULES_THREAD_THREAD_EX_H_ #define MODULES_THREAD_THREAD_EX_H_ +uint32_t ThreadPrepareState(void *JobDetails); uint32_t ThreadPreSegmentState(void *JobDetails); -uint32_t ThreadSegmentState(void *JobDetails); +uint32_t ThreadSegmentState(void *JobDetails, int SegmentId); uint32_t ThreadEndState(void *JobDetails); uint32_t ThreadInitialTestStub(); //uint32_t MotorPidRequestMessage(HardwarePidControl* request); +uint32_t Winder_Init(void); +uint32_t Winder_Prepare(void); +uint32_t Winder_Presegment(void *JobDetails); #endif diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c index 1727c650a..6c97c2e8f 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c @@ -77,7 +77,7 @@ uint32_t MotorsConfigMessage(HardwareMotor * request) MotorDriverConfig.HasConfigWord = MotorsCfg[Motor_i].has_configword; MotorDriverConfig.ConfigWord = MotorsCfg[Motor_i].configword; -// status = MotorConfig( Motor_i, &MotorDriverConfig); + status = MotorConfig( Motor_i, &MotorDriverConfig); // if (Motor_i == MOTOR_RDRIVING) // ThreadInitialTestStub(request); diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index bb866d608..2af3a699d 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -6,6 +6,7 @@ **************************************************************************************************************************/ #include "include.h" #include "thread.h" +#include "thread_ex.h" #include "../control/control.h" #include "../control/pidalgo.h" #include "PMR/Hardware/HardwareMotor.pb-c.h" @@ -44,10 +45,55 @@ typedef struct MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM]; uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM]; + +uint32_t PreviousPosition = 0, CurrentPosition = 0; +double totalLength = 0.0; +double CurrentRequestedLength = 0.0; +double CurrentProcessedLength = 0.0; + +typedef void (* ProcessedLengthFunc)(void); +ProcessedLengthFunc ProcessedLengthFuncPtr = NULL; +// segment/intersegment/distance to spool finished +void ThreadSegmentEnded(void); +void ThreadInterSegmentEnded(void); +void ThreadDistanceToSpoolEnded(void); ////////////////////////Slow Motor State//////////////////////////////////// -uint32_t ThreadPreSegmentState(void *JobDetails); +//uint32_t ThreadPreSegmentState(void *JobDetails); //////////////////////////////////////////////////////////////////////////// +/******************************************************************** +* +* Name : GTIME_Delta_Time_Pass +* +* Parameters : start_time. +* +* Return : time pass from start time +* +* Description : +* +*********************************************************************/ + +uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read) +{ + uint32_t Time_Pass; + #define MAX_COUNTER 0x3FFF //14 bits + + + if (Current_Read < Previous_Read) + Time_Pass = (MAX_COUNTER - Previous_Read) + Current_Read + 1; + else + Time_Pass = Current_Read - Previous_Read; + + return (Time_Pass); +} +/***************************************************************************************** + * + * + * + * + * + * + * **************************************************************************************/ uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { @@ -89,6 +135,46 @@ 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 @@ -97,7 +183,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) //read value is the dancer angle int i,index=MAX_THREAD_MOTORS_NUM; - int Pid_Id,DancerId; + int DancerId; int32_t TranslatedReadValue, avreageSampleValue = 0; double NormalizedError; if (IfIndex>>8 != IfTypeThread) @@ -121,7 +207,6 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) if(MotorControlConfig[index].m_isEnabled ) { - Pid_Id = ThreadMotorIdToControlId[index]; DancerId = ThreadMotorIdToDancerId[index]; TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue); @@ -170,6 +255,7 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request) ThreadPreSegmentState(request); return OK; } +bool InitialProcess = false; //******************************************************************************************************************** uint32_t ThreadPrepareState(void *JobDetails) { @@ -198,7 +284,9 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request) #ifdef DEBUG_TEST_FUNCTIONS 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(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i); + ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadLengthCBFunction, eOneMillisecond,MotorGetPositionFromFPGA,(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 not be controlled + ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eHundredMillisecond,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; @@ -215,7 +303,7 @@ uint32_t ThreadInitialTestStub(HardwareMotor * request) } Winder_Prepare(); //set 3 dancers to the profile positions - + InitialProcess = true; return OK; } @@ -244,18 +332,40 @@ uint32_t ThreadPreSegmentState(void *JobDetails) MotorSetSpeed(MOTOR_RLOADING, 1, 1); MotorSetSpeed(MOTOR_LLOADING, 1,1); + // activate control fr all motors //set speed for both rocker motors //wait for all motors to get to the required speed (set the target speed for the control to check) //call the job state machine when the thread system is ready - PreSegmentReady(Module_Thread,OK); + if ((InitialProcess==false) && JobTicket->enableintersegment == true) + { + ThreadUpdateProcessLength (JobTicket->intersegmentlength,(void *)ThreadInterSegmentEnded); + } + else + { + PreSegmentReady(Module_Thread,ModuleDone); + InitialProcess = false; + } return OK; } +void ThreadInterSegmentEnded(void) +{ + PreSegmentReady(Module_Thread,ModuleDone); +} +void ThreadSegmentEnded(void) +{ + SegmentReady(Module_Thread,ModuleDone); +} +void ThreadDistanceToSpoolEnded(void) +{ +} //******************************************************************************************************************** - uint32_t ThreadSegmentState(void *JobDetails) +uint32_t ThreadSegmentState(void *JobDetails, int SegmentId) { + JobTicket* JobTicket = JobDetails; + ThreadUpdateProcessLength (JobTicket->segments[SegmentId]->length,(void *)ThreadSegmentEnded); return OK; } |