diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c | 195 |
1 files changed, 189 insertions, 6 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c index 852c19a1a..aeacde455 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c @@ -36,6 +36,10 @@ #include "PMR/Stubs/StubHeatingTestResponse.pb-c.h" #include "PMR/Stubs/StubHeatingTestPollRequest.pb-c.h" #include "PMR/Stubs/StubHeatingTestPollResponse.pb-c.h" +#include "PMR/ThreadLoading/StartThreadLoadingRequest.pb-c.h" +#include "PMR/ThreadLoading/StartThreadLoadingResponse.pb-c.h" +#include "PMR/ThreadLoading/ContinueThreadLoadingRequest.pb-c.h" +#include "PMR/ThreadLoading/ContinueThreadLoadingResponse.pb-c.h" //#include <stdint.h> //#include <stdbool.h> @@ -62,6 +66,8 @@ THREAD_LOAD_STAGES_ENUM LoadStages = THREAD_LOAD_INIT; ProcessParameters ProcessParametersClear,ProcessParametersRecover; + uint32_t LoadStatus = OK; + char LoadErrorMsg[100]; int32_t keepmicrostep; int32_t keepkvalrun,keepkvalhold; uint8_t CallbackCounter = 0; @@ -70,10 +76,16 @@ uint32_t UnloadingStart = 0; uint8_t NumberOfDrierLoaderCycles = 0; uint32_t status = OK; - uint32_t LoadingControlId = 0xFF,PullerControlId = 0xFF,WinderControlId = 0xFF; + uint32_t LoadingControlId = 0xFF,PullerControlId = 0xFF,WinderControlId = 0xFF, SpeedTControlId = 0xFF; + uint32_t previousPosition = 0, currentPosition = 0,Tinitialpos = 0xFFFF; + uint32_t LoadCounter = 0; + double TotalProcessedLen = 0.0; + double LengthCalculationMultiplier; uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM LoadStages); uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue); + uint32_t ThreadLoadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue); uint32_t Thread_Load_Dryer_UnLoading(void); + uint32_t ThreadLoadingReport(void); typedef struct { @@ -104,13 +116,17 @@ //FRESULT Fresult = FR_OK; REPORT_MSG(LoadStages, "Thread Load State Machine step"); - LoadArmInfo.LoadArmBackLash = 5; + LoadStatus = OK; + LoadArmInfo.LoadArmBackLash = MCU_E2PromDryerBacklashRead(); + REPORT_MSG(LoadArmInfo.LoadArmBackLash, "Thread Load MCU_E2PromDryerBacklashRead"); + if (LoadArmInfo.LoadArmBackLash > 10) + LoadArmInfo.LoadArmBackLash = 5; //LoadArmInfo.LoadArmRounds = 0xFF; //EEPROM_STORAGE_DRYER_CYCLES MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmInfo.LoadArmRounds); + if (LoadArmInfo.LoadArmRounds <= 2) LoadArmInfo.LoadArmRounds = 20; - MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmInfo.LoadArmRounds); Report("MotorMovetoEncoderPosition",__FILE__,__LINE__,LoadArmInfo.LoadArmRounds,RpWarning,LoadArmInfo.LoadArmBackLash,0); /*Fresult = f_mkdir(SW_INFO_DIR); @@ -156,6 +172,8 @@ if (HandleProcessParameters(&ProcessParametersClear,false)!= OK) { LOG_ERROR (LoadStages, "Thread_Load_Init failed"); + strcpy(LoadErrorMsg,"Thread_Load_Init failed"); + LoadStatus = ERROR; } else { @@ -294,6 +312,9 @@ LOG_ERROR(LoadStages,"Load sequence timeout"); TimeoutsCounter = 0; load.color = fastBILNK; + usnprintf(LoadErrorMsg, 100, "Load sequence timeout %d motor %d",LoadStages, MotorId); + LoadStatus = ERROR; + ThreadLoadingReport(); } else { @@ -310,6 +331,7 @@ else { load.color = fastBILNK; + ThreadLoadingReport(); } } } @@ -472,6 +494,13 @@ RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction ); LoadingControlId = 0xFF; } + if (SpeedTControlId != 0xFF) + { + RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction ); + SpeedTControlId = 0xFF; + } + Report("Total Feeder load",__FILE__,__LINE__,dryerbufferCentimeters,RpMessage,TotalProcessedLen,0); + if (CallbackCounter) { CallbackCounter--; @@ -546,6 +575,15 @@ LoadingControlId = AddControlCallback(NULL,ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR); Report("AddControlCallback feeder",__FILE__,__LINE__,LoadingControlId,RpMessage,IfTypeThread*0x100+FEEDER_MOTOR,0); + if (SpeedTControlId != 0xFF) + { + RemoveControlCallback(SpeedTControlId,ThreadLoadLengthCBFunction); + SpeedTControlId = 0xFF; + } + //SetMotHome(ThreadMotorIdToMotorId[Motor_i]); + LengthCalculationMultiplier = (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].pulleyradius*2*PI)/(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].pulseperround*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].microstep); + SpeedTControlId = AddControlCallback(NULL,ThreadLoadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+HARDWARE_MOTOR_TYPE__MOTO_RDRIVING),HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,0); + Tinitialpos = 0xFFFF;previousPosition = 0; currentPosition = 0; CallbackCounter++; Report("Thread_Load_Dryer_Loading",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, @@ -596,6 +634,11 @@ RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction ); WinderControlId = 0xFF; } + if (SpeedTControlId != 0xFF) + { + RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction ); + SpeedTControlId = 0xFF; + } MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_RLOADING); @@ -673,12 +716,10 @@ // numberOfSteps, Thread_Load_Dryer_Loading_Callback, 100000); UnloadingStart = msec_millisecondCounter; Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,UnloadingStart,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); - LoadArmInfo.LoadArmBackLash = 5; status |= MotorMoveToStopper(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize), MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/2, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000); //Keep Notation How Many Rotations In The Dryer - //LoadArmInfo.LoadArmBackLash = 0; LoadArmInfo.LoadArmRounds = (int)dryerbufferlength; load.color = BLINK; return OK; @@ -752,6 +793,7 @@ case THREAD_LOAD_JOG_THREAD: //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING //LoadStages++; Thread_Load_Jog_Thread(); + ThreadLoadingReport(); break; case THREAD_LOAD_END: LoadStages = THREAD_LOAD_END; @@ -770,6 +812,11 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue) Report("ThreadLoadButton called on job",__FILE__,__LINE__,LoadStages,RpMessage,ReadValue,0); return ERROR; } + if (CallbackCounter) + { + Report("ThreadLoadButton called when the loading is active",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0); + return ERROR; + } Report("ThreadLoadButton",__FILE__,__LINE__,LoadStages,RpMessage,ReadValue,0); if (LoadStages > ReadValue) { @@ -781,8 +828,60 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue) } return OK; } -uint32_t LoadCounter = 0; +uint32_t ThreadLoadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue) +{ + uint32_t positionDiff = 0,prevprev; + double length = 0.0; + char lenstr[150]; + + int index = MAX_THREAD_MOTORS_NUM; + if (IfIndex>>8 != IfTypeThread) + { + LOG_ERROR (IfIndex, "Wrong Interface type"); + return 0xFFFFFFFF; + } + index = IfIndex&0xFF; +// if (CurrentRequestedLength == 0.0) +// return OK; + if (index != HARDWARE_MOTOR_TYPE__MOTO_RDRIVING) + { + LOG_ERROR (IfIndex, "Wrong Motor"); + return 0xFFFFFFFF; + } + currentPosition = MotorGetPosition(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING); + if (currentPosition != 0) + { + if (Tinitialpos == 0xFFFF) + { + previousPosition = currentPosition; + Tinitialpos = 0; + } + prevprev = previousPosition; + 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) + + //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep; + length = (double)(positionDiff)*LengthCalculationMultiplier; + + //if (length > 1000) + { + usnprintf(lenstr, 100, " length %d, diff 0x%x, pos 0x%x prev 0x%x",(int)length*100,(int)positionDiff,previousPosition,prevprev); + SendJobProgress(0.0,0,false, lenstr); + ReportWithPackageFilter(ThreadFilter,lenstr,__FILE__,__LINE__,(int)TotalProcessedLen,RpWarning,(int)TotalProcessedLen,0); + length = 0; + + } + + } + TotalProcessedLen += length; + + return OK; +} uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //read value is the dancer angle @@ -911,6 +1010,7 @@ void ThreadLoadRequest(MessageContainer* requestContainer) // return ERROR; //#endif } + void ThreadLoadPollRequest(MessageContainer* requestContainer) { uint8_t* container_buffer; @@ -936,3 +1036,86 @@ void ThreadLoadPollRequest(MessageContainer* requestContainer) SendChars(container_buffer, container_size); stub_heating_test_poll_request__free_unpacked(request,NULL); } +char ThreadLoadingToken[36+1] = {0}; +int MessageState = 0; //0 - none, 1 = start, 2 = continue +uint32_t ThreadLoadingReport(void) +{ + MessageContainer responseContainer; + bool last = false; + StartThreadLoadingResponse response = START_THREAD_LOADING_RESPONSE__INIT; + ContinueThreadLoadingResponse Cresponse = CONTINUE_THREAD_LOADING_RESPONSE__INIT; + + if (ThreadLoadingToken[0] == 0) + return OK; + + if (MessageState == 1) + { + response.has_state = true; + if (LoadStatus == OK) + { + response.state = THREAD_LOADING_STATE__Preparing; + if (LoadStages == THREAD_LOAD_INITIAL_TENSION) + { + response.state = THREAD_LOADING_STATE__ReadyForLoading; + last = true; + } + } + else + { + response.state = THREAD_LOADING_STATE__PreparationError; + response.errorreason = LoadErrorMsg; + } + responseContainer = createContainer(MESSAGE_TYPE__StartThreadLoadingResponse, ThreadLoadingToken, last, &response, &start_thread_loading_response__pack, &start_thread_loading_response__get_packed_size); + responseContainer.continuous = true; + } + else if (MessageState == 2) + { + Cresponse.has_state = true; + if (LoadStatus == OK) + { + Cresponse.state = THREAD_LOADING_STATE__Finalizing; + if (LoadStages >= THREAD_LOAD_JOG_THREAD) + { + Cresponse.state = THREAD_LOADING_STATE__Completed; + last = true; + } + } + else + { + Cresponse.state = THREAD_LOADING_STATE__FinalizationError; + Cresponse.errorreason = LoadErrorMsg; + } + responseContainer = createContainer(MESSAGE_TYPE__ContinueThreadLoadingResponse, ThreadLoadingToken, last, &Cresponse, &continue_thread_loading_response__pack, &continue_thread_loading_response__get_packed_size); + responseContainer.continuous = true; + } + else return ERROR; + + //------------------------------------------------------------------------------------------- + uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer)); + size_t container_size = message_container__pack(&responseContainer, container_buffer); + my_free(responseContainer.data.data); + SendChars((char*)container_buffer, container_size); + + return OK; + +} + +uint32_t StartThreadLoadingFunc(MessageContainer* requestContainer) +{ + StartThreadLoadingRequest *request = start_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); + ustrncpy (ThreadLoadingToken, requestContainer->token,36); + MessageState = 1; + ThreadLoadingReport(); + return OK; + +} + +uint32_t ContinueThreadLoadingFunc(MessageContainer* requestContainer) +{ + ContinueThreadLoadingRequest *request = continue_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); + ustrncpy (ThreadLoadingToken, requestContainer->token,36); + MessageState = 2; + ThreadLoadingReport(); + return OK; + +} |