diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c | 95 |
1 files changed, 86 insertions, 9 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c index 2bdf1b17f..98147f6b2 100644 --- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c +++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c @@ -696,7 +696,8 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) }*`/ }*/ calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index]; - calculated_speed = calculated_speed*InitialDryerSpeed/OriginalMotorSpd_2PPS[DRYER_MOTOR]; + if (index != WINDER_MOTOR) + calculated_speed = calculated_speed*InitialDryerSpeed/OriginalMotorSpd_2PPS[DRYER_MOTOR]; //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index]; if (0)//(JobCounter % 1000 == 0) //if (JobCounter < 100)//(FirstCalcInJob == true) @@ -728,6 +729,14 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue) }*/ CurrentControlledSpeed[index] = calculated_speed; MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed); +#ifdef FOUR_WINDERS + if (index == WINDER_MOTOR) + { + MotorSetSpeed(Winder_2_Motor, calculated_speed); + MotorSetSpeed(Winder_3_Motor, calculated_speed); + MotorSetSpeed(Winder_4_Motor, calculated_speed); + } +#endif } else { @@ -841,19 +850,21 @@ uint32_t Release_Right_TFU_TensionCallback(uint32_t deviceID, uint32_t BusyFlag) uint32_t Release_Right_TFU_Tension() { uint32_t status = OK; +#ifndef FOUR_WINDERS if (RTFU_Up == true) { Report("Release_Right_TFU_Tension",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4,0); RTFU_Up = false; status = MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].microstep, Release_Right_TFU_TensionCallback,1000); } - +#endif return status; } int SecondFeederCorrection = 4; int PrepareWaitCount = 0; uint32_t Adjust_Right_TFU_Tension_2nd_Callback(uint32_t MotorId, uint32_t ReadValue) { +#ifndef FOUR_WINDERS MotorStop (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,Soft_Stop); //per L6470 errata between mov and run commands Report("Adjust_Right_TFU_Tension_2ndCallback x more steps",__FILE__,__LINE__,MotorId,RpMessage,SecondFeederCorrection,0); if (JobIsActive()==false) @@ -872,20 +883,23 @@ uint32_t Adjust_Right_TFU_Tension_2nd_Callback(uint32_t MotorId, uint32_t ReadVa ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback Prepare Ready",__FILE__,__LINE__,2,RpWarning,PrepareWaitCount,0); PrepareReady(Module_Thread,ModuleDone); } - +#endif return OK; } uint32_t Adjust_Right_TFU_Tension_Callback(uint32_t MotorId, uint32_t ReadValue) { +#ifndef FOUR_WINDERS Report("Adjust_Right_TFU_Tension_Callback",__FILE__,__LINE__,MotorId,RpMessage,0,0); MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,SecondFeederCorrection* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].microstep, Adjust_Right_TFU_Tension_2nd_Callback,1000); RTFU_Up = true; +#endif return OK; } uint32_t Adjust_Right_TFU_Tension(double tension) { uint32_t status = OK; +#ifndef FOUR_WINDERS if (tension > 0.5) //0 = lower position, 1 = high position { if (FPGA_Read_limit_Switches(GPI_LS_RDANCER_UP) == NO_LIMIT) @@ -895,7 +909,7 @@ uint32_t Adjust_Right_TFU_Tension(double tension) Report("Adjust_Right_TFU_Tension",__FILE__,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,PrepareWaitCount,0); } } - +#endif return status; } uint32_t ThreadPrepare_TensionCallback (int MotorId, double tension) @@ -1013,11 +1027,11 @@ uint32_t ThreadPrepareState(void *JobDetails) PrepareWaitCount = 0; status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, windertension); - ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Winder",__FILE__,HARDWARE_DANCER_TYPE__LeftDancer,PrepareWaitCount,RpFatalError,(int)windertension,0); + ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Winder",__FILE__,HARDWARE_DANCER_TYPE__LeftDancer,PrepareWaitCount,RpWarning,(int)windertension,0); status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__MiddleDancer, pullertension); - ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Puller",__FILE__,HARDWARE_DANCER_TYPE__MiddleDancer,PrepareWaitCount,RpFatalError,(int)pullertension,0); + ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Puller",__FILE__,HARDWARE_DANCER_TYPE__MiddleDancer,PrepareWaitCount,RpWarning,(int)pullertension,0); status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__RightDancer, feedertension); - ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Feeder",__FILE__,HARDWARE_DANCER_TYPE__RightDancer,PrepareWaitCount,RpFatalError,(int)feedertension,0); + ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Feeder",__FILE__,HARDWARE_DANCER_TYPE__RightDancer,PrepareWaitCount,RpWarning,(int)feedertension,0); FirstCalcInJob = true; if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false) @@ -1034,14 +1048,14 @@ uint32_t ThreadPrepareState(void *JobDetails) /*if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) != LIMIT) { - ReportWithPackageFilter(ThreadFilter,"Dyeing head is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpFatalError,LIMIT,0); + ReportWithPackageFilter(ThreadFilter,"Dyeing head is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpWarning,LIMIT,0); //JobEndReason = JOB_LIDS_OPEN; //PrepareReady(Module_Thread,ModuleFail); //return ERROR; } if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) != LIMIT) { - ReportWithPackageFilter(ThreadFilter,"Dryer lid is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpFatalError,LIMIT,0); + ReportWithPackageFilter(ThreadFilter,"Dryer lid is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpWarning,LIMIT,0); //JobEndReason = JOB_LIDS_OPEN; //PrepareReady(Module_Thread,ModuleFail); //return ERROR; @@ -1120,6 +1134,7 @@ uint32_t ThreadPrepareState(void *JobDetails) ///////////////////////////////////////////////////// MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize); +#ifndef FOUR_WINDERS 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 { ReportWithPackageFilter(ThreadFilter,"Feeder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); @@ -1132,6 +1147,7 @@ uint32_t ThreadPrepareState(void *JobDetails) LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); SpeedControlId = AddControlCallback(NULL,ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); } +#endif 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 { ReportWithPackageFilter(ThreadFilter,"Puller Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0); @@ -1145,6 +1161,7 @@ uint32_t ThreadPrepareState(void *JobDetails) PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep); PoolerSpeedControlId = AddControlCallback(NULL,PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); } +#ifndef FOUR_WINDERS 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) @@ -1159,6 +1176,7 @@ uint32_t ThreadPrepareState(void *JobDetails) //AddControlCallback(NULL,ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i); #endif } +#endif 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) @@ -1196,7 +1214,57 @@ uint32_t ThreadPrepareState(void *JobDetails) //set 3 dancers to the profile positions return OK; } +uint32_t UpdatePidDuringRun(HardwarePidControl *request) +{ + int Motor_i = MAX_THREAD_MOTORS_NUM,i; + double temp_dt; + for (i=0;i<MAX_THREAD_MOTORS_NUM;i++) + { + if (ThreadMotorIdToControlId[i] == request->hardwarepidcontroltype) + { + Motor_i = i; + break; + } + } + if (Motor_i == MAX_THREAD_MOTORS_NUM) + return ERROR; + + + if (request->derivativetime == true) + { + MotorControlConfig[Motor_i].m_params.Kd = request->derivativetime; + ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kd",__FILE__,Motor_i,(int)(request->derivativetime),RpWarning,0,0); + } + if (request->proportionalgain == true) + { + MotorControlConfig[Motor_i].m_params.Kp = request->proportionalgain; + ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kp",__FILE__,Motor_i,(int)(request->proportionalgain),RpWarning,0,0); + } + if (request->integraltime == true) + { + MotorControlConfig[Motor_i].m_params.Ki = request->integraltime; + ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Ki",__FILE__,Motor_i,(int)(request->integraltime),RpWarning,0,0); + } + if (request->epsilon == true) + { + MotorControlConfig[Motor_i].m_params.epsilon = request->epsilon; + ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun epsilon",__FILE__,Motor_i,(int)(request->epsilon*10000),RpWarning,0,0); + } + if (request->has_controloutputtype == true) + { + MotorControlConfig[Motor_i].m_params.dt = request->controloutputtype; + temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001; + MotorTiming[Motor_i] = (int)temp_dt; + if (MotorTiming[Motor_i]) + { + MotorTimer[Motor_i] = MotorTiming[Motor_i]-1; + } + ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun dt",__FILE__,Motor_i,(int)(request->controloutputtype*1000),RpWarning,temp_dt,0); + } +////////////////////////////////////////////////// + return OK; +} void SetOriginMotorSpeed(float process_speed) { int Motor_i, HW_Motor_Id; @@ -1451,6 +1519,15 @@ char Endstr[150]; for ( Motor_i = 0;Motor_i <= WINDER_MOTOR;Motor_i++) { MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz); +#ifdef FOUR_WINDERS + if (Motor_i == WINDER_MOTOR) + { + MotorStop(Winder_2_Motor, Hard_Hiz); + MotorStop(Winder_3_Motor, Hard_Hiz); + MotorStop(Winder_4_Motor, Hard_Hiz); + } +#endif + } MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz); MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz); |