diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c | 159 |
1 files changed, 112 insertions, 47 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c index edfa3b0b9..1c28a5447 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c @@ -35,26 +35,26 @@ //#include <stdint.h> //#include <stdbool.h> - typedef enum +/* typedef enum { - THREAD_LOAD_INIT, - THREAD_LOAD_REDUCE_HEAT, //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW, - THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,//USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY - THREAD_LOAD_CENTER_HEAD_ROCKERS, //puthead cleaning rockers to middle position - THREAD_LOAD_OPEN_COVERS, //OPEN DYEING HEAD COVER AND DRYER LID - THREAD_LOAD_LIFT_DANCERS, - THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE - THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND - THREAD_LOAD_CLOSE_ROCKERS, - THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD - THREAD_LOAD_CLOSE_LIDS, - THREAD_LOAD_RESUME_HEATING, - THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,//JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION - THREAD_LOAD_DRYER_LOADING, //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40 + 0 THREAD_LOAD_INIT, + 1 THREAD_LOAD_REDUCE_HEAT, //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW, + 2 THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,//USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY + 3 THREAD_LOAD_CENTER_HEAD_ROCKERS, //puthead cleaning rockers to middle position + 4 THREAD_LOAD_OPEN_COVERS, //OPEN DYEING HEAD COVER AND DRYER LID + 5 THREAD_LOAD_LIFT_DANCERS, + 6 THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE + 7 THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND + 8 THREAD_LOAD_CLOSE_ROCKERS, + 9 THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD + 10 THREAD_LOAD_CLOSE_LIDS, + 11 THREAD_LOAD_RESUME_HEATING, + 12 THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,//JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION + 13 THREAD_LOAD_DRYER_LOADING, //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40 //KEEP NOTATION HOW MANY ROTATIONS IN THE DRYER - THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING - THREAD_LOAD_END - }THREAD_LOAD_STAGES_ENUM; + 14 THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING + 15 THREAD_LOAD_END + }THREAD_LOAD_STAGES_ENUM;*/ THREAD_LOAD_STAGES_ENUM LoadStages = THREAD_LOAD_INIT; ProcessParameters ProcessParametersClear,ProcessParametersRecover; uint8_t NumberOfDrierLoaderCycles = 0; @@ -69,7 +69,7 @@ uint32_t LoadArmBackLash; }LoadArmInfoStruc; LoadArmInfoStruc LoadArmInfo; - char LoadArmPath[50] = "0://SWInfo//LoadArm.msg"; + char LoadArmPath[50] = "0://SysInfo//LoadArm.cfg"; //RUN MOTOR TO SWITCH WITH TIMEOUT //RUN MOTOR TO BREAK SENSOR @@ -85,7 +85,7 @@ FRESULT Fresult = FR_OK; REPORT_MSG(LoadStages, "Thread Load State Machine step"); - LoadArmInfo.LoadArmBackLash = 40; + LoadArmInfo.LoadArmBackLash = 10; LoadArmInfo.LoadArmRounds = 0xFF; Fresult = f_mkdir(SW_INFO_DIR); if ((Fresult == FR_OK)||(Fresult == FR_EXIST)) @@ -100,10 +100,11 @@ } } + Report("Thread_Load_Init",__FILE__,__LINE__,LoadArmInfo.LoadArmBackLash,RpMessage,LoadArmInfo.LoadArmRounds,0); memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters)); //NumberOfDrierLoaderCycles = loadLoadArmParameters(); LoadStages++; - //ThreadLoadStateMachine(LoadStages); + ThreadLoadStateMachine(LoadStages); return OK; } @@ -145,17 +146,18 @@ { // Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); - NumberOfDrierLoaderCycles--; + /*NumberOfDrierLoaderCycles--; if (NumberOfDrierLoaderCycles) { Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize), MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000); } - else + else*/ { Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); //storeLoadArmParameters(); + SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); //set this point as the spool home LoadStages++; //ThreadLoadStateMachine(LoadStages); } @@ -168,18 +170,23 @@ NumberOfDrierLoaderCycles=0; //storeLoadArmParameters(); LoadStages++; + SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM); //set this point as the spool home + //ThreadLoadStateMachine(LoadStages); return OK; } uint32_t Thread_Load_Set_Load_Arm_To_Start_Position(void) { + uint32_t numberOfSteps = 0; REPORT_MSG(LoadStages, "Thread Load State Machine step"); if (LoadArmInfo.LoadArmRounds != 0xFF) { - Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); + //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40 + numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; + Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,numberOfSteps,RpMessage,NumberOfDrierLoaderCycles,0); NumberOfDrierLoaderCycles = LoadArmInfo.LoadArmRounds; status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize), - MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000); + numberOfSteps, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000); } else //number of circles is not known - compare to position of the motor to verify location at the stopper { @@ -196,18 +203,16 @@ { Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); REPORT_MSG(MotorId, "Thread_Load_HomingCallback Motor Id"); - if(ControlId != 0xFF) - { - RemoveControlCallback(ControlId, ThreadLoadControlCBFunction ); - ControlId = 0xFF; - } if (CallbackCounter) { CallbackCounter--; if (CallbackCounter == 0) { LoadStages++; - //ThreadLoadStateMachine(LoadStages); + if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call + { + //ThreadLoadStateMachine(LoadStages); + } } } else @@ -241,9 +246,9 @@ { REPORT_MSG(LoadStages, "Thread Load State Machine step"); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 20, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,10000); + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,15000); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, 200, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2], Thread_Load_HomingCallback,10000); + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,20000); return OK; } @@ -252,9 +257,9 @@ { REPORT_MSG(LoadStages, "Thread Load State Machine step"); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 200, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,10000); + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 700, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,10000); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 200, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,10000); + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 700, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,10000); return OK; } @@ -276,9 +281,9 @@ { REPORT_MSG(LoadStages, "Thread Load State Machine step"); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 200, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,10000); + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 500, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,10000); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 200, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,10000); + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 300, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,10000); return OK; } @@ -288,9 +293,9 @@ { REPORT_MSG(LoadStages, "Thread Load State Machine step"); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 200, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,10000); + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,4000); CallbackCounter++; - MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, 200, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2], Thread_Load_HomingCallback,10000); + MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,10000); return OK; } @@ -333,15 +338,68 @@ MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 200, FEEDER_DANCER ,true, Thread_Load_HomingCallback,10000); return OK; } + + uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue) + { + Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0); + REPORT_MSG(MotorId, "Thread_Load_Dryer_Loading_Callback Motor Id"); + if(ControlId != 0xFF) + { + MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz); + RemoveControlCallback(ControlId, ThreadLoadControlCBFunction ); + ControlId = 0xFF; + } + if (CallbackCounter) + { + CallbackCounter--; + if (CallbackCounter == 0) + { + LoadStages++; + if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call + { + //ThreadLoadStateMachine(LoadStages); + } + } + } + else + LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0"); + return OK; + } + uint32_t Thread_Load_Dryer_Loading(void) { REPORT_MSG(LoadStages, "Thread Load State Machine step"); LoadArmInfo.LoadArmRounds = 20; + uint32_t numberOfSteps = 0; //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40 - OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000; + SetOriginMotorSpeed(19); +// OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000; +// CurrentControlledSpeed[FEEDER_MOTOR] = 1000; + + numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius; + MotorControlConfig[FEEDER_MOTOR].m_params.MAX = 1; + MotorControlConfig[FEEDER_MOTOR].m_params.MIN = MotorsControl[FEEDER_MOTOR].outputproportionalpowerlimit*-1; + MotorControlConfig[FEEDER_MOTOR].m_params.Kd = MotorsControl[FEEDER_MOTOR].derivativetime; + MotorControlConfig[FEEDER_MOTOR].m_params.Kp = MotorsControl[FEEDER_MOTOR].proportionalgain; + MotorControlConfig[FEEDER_MOTOR].m_params.Ki = MotorsControl[FEEDER_MOTOR].integraltime; + MotorControlConfig[FEEDER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[FEEDER_MOTOR].setpointramprateorsoftstartramp; + MotorControlConfig[FEEDER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[FEEDER_MOTOR].outputonoffhysteresisvalue; + MotorControlConfig[FEEDER_MOTOR].m_params.epsilon = MotorsControl[FEEDER_MOTOR].epsilon; + MotorControlConfig[FEEDER_MOTOR].m_params.dt = MotorsControl[FEEDER_MOTOR].controloutputtype; + MotorControlConfig[FEEDER_MOTOR].m_ingnoreValue = MotorsControl[FEEDER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses + MotorControlConfig[FEEDER_MOTOR].m_calculatedError = 0; + MotorControlConfig[FEEDER_MOTOR].m_integral = 0; + MotorControlConfig[FEEDER_MOTOR].m_isEnabled = true; + MotorControlConfig[FEEDER_MOTOR].m_isReady = true; + MotorControlConfig[FEEDER_MOTOR].m_mesuredParam = 0; + MotorControlConfig[FEEDER_MOTOR].m_preError = 0; + MotorControlConfig[FEEDER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage + MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize); + ControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR); + CallbackCounter++; MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize, - MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds, Thread_Load_HomingCallback, 100000); + numberOfSteps, Thread_Load_Dryer_Loading_Callback, 100000); //Keep Notation How Many Rotations In The Dryer //LoadArmInfo.LoadArmBackLash = 0; @@ -352,7 +410,7 @@ uint32_t Thread_Load_Jog_ThreadStop(uint32_t index, uint32_t ReadValue) { REPORT_MSG(LoadStages, "Thread Load State Machine step"); - RemoveControlCallback(ControlId, Thread_Load_Jog_ThreadStop ); + SafeRemoveControlCallback(ControlId, Thread_Load_Jog_ThreadStop ); ControlId = 0xFF; ThreadAbortJoggingFunc(); LoadStages++; @@ -363,7 +421,7 @@ //Jog Thread Shortly To Make Sure Spool Is Running. Report End Of Loading { REPORT_MSG(LoadStages, "Thread Load State Machine step"); - ThreadJoggingFunc(40); + ThreadJoggingFunc(20); ControlId = AddControlCallback(Thread_Load_Jog_ThreadStop, eOneSecond*5,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR); return OK; } @@ -449,17 +507,18 @@ return OK; } -uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue) +uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) { //read value is the dancer angle int i; int DancerId; int32_t TranslatedReadValue, avreageSampleValue = 0; + int index=MAX_THREAD_MOTORS_NUM; //double tempcalcspeed = 0; uint32_t calculated_speed; double NormalizedError; - DancerId = ThreadMotorIdToDancerId[index]; + DancerId = FEEDER_DANCER;//ThreadMotorIdToDancerId[index]; if (ReadValue < 10) { Report("Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0); @@ -470,11 +529,17 @@ uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue) return OK; } TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint; - if (index == POOLER_MOTOR) + if (IfIndex>>8 != IfTypeThread) + { + LOG_ERROR (IfIndex, "Wrong Interface type"); + return 0xFFFFFFFF; + } + index = IfIndex&0xFF; + /*if (index == POOLER_MOTOR) { //pooler dancer is right sided: data is opposite TranslatedReadValue = (-1*TranslatedReadValue); - } + }*/ //TranslatedReadValue = 0;//test MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue); MotorSamplePointer[index]++; |